Ray transformation in a Ray - OBB intersection test - c++

I've implemented an algorithm that tests for a Ray - AABB intersection and it works fine. But when I try to transform Ray to the AABB's local space (making this a Ray - OBB test), I can't get correct results. I've studied several forums and other resources, but still missing something. (Some sources suggesting to apply inverted transformation to the ray origin and its end, and only then calc. direction, other - to apply transformation to origin and direction). Can someone point in the right direction (no pun intended)?
Here goes two functions responsible for the math:
1) Calculating inverses and other things to perform tests
bool Ray::intersectsMesh(const Mesh& mesh, const Transformation& transform) {
float largestNearIntersection = std::numeric_limits<float>::min();
float smallestFarIntersection = std::numeric_limits<float>::max();
glm::mat4 modelTransformMatrix = transform.modelMatrix();
Box boundingBox = mesh.boundingBox();
glm::mat4 inverse = glm::inverse(transform.modelMatrix());
glm::vec4 newOrigin = inverse * glm::vec4(mOrigin, 1.0);
newOrigin /= newOrigin.w;
mOrigin = newOrigin;
mDirection = glm::normalize(inverse * glm::vec4(mDirection, 0.0));
glm::vec3 xAxis = glm::vec3(glm::column(modelTransformMatrix, 0));
glm::vec3 yAxis = glm::vec3(glm::column(modelTransformMatrix, 1));
glm::vec3 zAxis = glm::vec3(glm::column(modelTransformMatrix, 2));
glm::vec3 OBBTranslation = glm::vec3(glm::column(modelTransformMatrix, 3));
printf("trans x %f y %f z %f\n", OBBTranslation.x, OBBTranslation.y, OBBTranslation.z);
glm::vec3 delta = OBBTranslation - mOrigin;
bool earlyFalseReturn = false;
calculateIntersectionDistances(xAxis, delta, boundingBox.min.x, boundingBox.max.x, &largestNearIntersection, &smallestFarIntersection, &earlyFalseReturn);
if (smallestFarIntersection < largestNearIntersection || earlyFalseReturn) { return false; }
calculateIntersectionDistances(yAxis, delta, boundingBox.min.y, boundingBox.max.y, &largestNearIntersection, &smallestFarIntersection, &earlyFalseReturn);
if (smallestFarIntersection < largestNearIntersection || earlyFalseReturn) { return false; }
calculateIntersectionDistances(zAxis, delta, boundingBox.min.z, boundingBox.max.z, &largestNearIntersection, &smallestFarIntersection, &earlyFalseReturn);
if (smallestFarIntersection < largestNearIntersection || earlyFalseReturn) { return false; }
return true;
}
2) Helper function (probably not needed here as its relates only to AABB tests and works fine)
void Ray::calculateIntersectionDistances(const glm::vec3& axis,
const glm::vec3& delta,
float minPointOnAxis,
float maxPointOnAxis,
float *largestNearIntersection,
float *smallestFarIntersection,
bool *earlyFalseRerutn)
{
float divident = glm::dot(axis, delta);
float denominator = glm::dot(mDirection, axis);
if (fabs(denominator) > 0.001f) {
float t1 = (divident + minPointOnAxis) / denominator;
float t2 = (divident + maxPointOnAxis) / denominator;
if (t1 > t2) { std::swap(t1, t2); }
*smallestFarIntersection = std::min(t2, *smallestFarIntersection);
*largestNearIntersection = std::max(t1, *largestNearIntersection);
} else if (-divident + minPointOnAxis > 0.0 || -divident + maxPointOnAxis < 0.0) {
*earlyFalseRerutn = true;
}
}

As it turned out, the ray's world -> model transformation was correct. The bug was in the intersection test. I had to completely replace the intersection code, because I wasn't able to identify the bug in the old code, unfortunately.
Ray transformation code:
glm::mat4 inverse = glm::inverse(transform.modelMatrix());
glm::vec4 start = inverse * glm::vec4(mOrigin, 1.0);
glm::vec4 direction = inverse * glm::vec4(mDirection, 0.0);
direction = glm::normalize(direction);
And the Ray - AABB test was stolen from here

Related

Ray vs ellipsoid intersection

I am trying to implement ray vs ellipsoid intersection by "squishing" space and doing ray vs sphere:
create mat3 S with ellipsoid radius at diagonal
squish ray by multiplying start and direction by an inverse of S
intersect ray with sphere of radius 1.0 in local space
multiply hitPoint by S to unsquish it.
Here is ray vs sphere:
float P = glm::dot(dir, sphereCenter-start);
float L = glm::distance(start, sphereCenter);
float d = sqrt(L*L - P*P);
if (d < radius) {
float x0 = sqrt(1.f - d*d);
hitPoint = start + dir*(P - x0);
hitNormal = glm::normalize(hitPoint - sphereCenter);
}
else if (d == radius) {
hitPoint = start + dir*P;
hitNormal = glm::normalize(hitPoint - sphereCenter);
}
else {
return false;
}
if (glm::distance(start, hitPoint) > dist) return false;
return true;
Here is the squishing part:
glm::vec3 S = start;
glm::vec3 Dir = dir;
auto sphereCenter = thisEntity()->transform()->getPosition();
auto scale = thisEntity()->transform()->getScale();
glm::mat3 q = glm::mat3(0);
float x = _radius.x * scale.x;
float y = _radius.y * scale.y;
float z = _radius.z * scale.z;
q[0][0] = x;
q[1][1] = y;
q[2][2] = z;
glm::mat3 qI = glm::inverse(q);
S = qI * S;
Dir = qI * Dir;
//calculate hit point in world space squished
glm::vec3 hitPoint, hitNormal;
if (!IntersectionsMath::instance()->segmentVsSphere(sphereCenter, S, Dir, dist, 1.f, hitPoint, hitNormal)) return;
hitPoint = q * hitPoint;
hit.pushHit(hitPoint, hitNormal, this);
Current ray sphere code is for world position, i'm trying to make it work at the origin so it shouldn't matter. Ray vs regular sphere works fine, ellipsoid is the problem.
I spent a lot of time on this and something somewhere is wrong.
Problem:
The center of scaling matters.
Solution:
Perform the scaling about the center of the ellipsoid.
... and not the origin as you are doing right now. This is because, although the direction of the ray will be the same (it is just a directional vector), the relative displacement between the scaled source and center of the sphere will be different:
Scaling about origin (current code):
Source S' = qI * S, center C' = qI * C --- S' - C' = qI * (S - C)
Scaling about ellipsoid center (correct procedure):
Source S" = qI * (S - C), center C" = C --- S" - C" = qI * (S - C) - C
The two displacements differ by the position of the original ellipsoid; thus your current ray will likely miss / give false positives.
Corrected code:
// scale about the ellipsoid's position by subtracting before multiplying
// more appropriate name would be "ellipseCenter" to avoid confusion
S_ = qI * (S - sphereCenter);
// this ::normalize should really be in the intersection function
Dir_ = glm::normalize(qI * Dir);
// calculate hit point in world space squished
// ... but around the origin in the squashed coordinate system
glm::vec3 hitPoint, hitNormal;
if (!IntersectionsMath::instance()->segmentVsSphere(
glm::vec3::ZERO, S_, Dir_,
dist, 1.f,
hitPoint, hitNormal)) return;
// re-apply the offset
hitPoint = q * hitPoint + sphereCenter
// problem: hitNormal will not be correct for the ellipsoid when scaled
// solution: divide through each component by square of respective semi-axis
// (will provide proof upon request)
hitNormal.x /= (x * x); hitNormal.y /= (y * y); hitNormal.z /= (z * z);

How to get the angle (pitch/yaw) between two 3D vectors for an autoaim

I'm trying to get the angles between two vectors (My Camera Position and Enemy Position) to create an autoaim/aimbot.
The game is Unity based, it uses the left handed coordinate system. X Y Z is right, up, forward.
The game also uses degrees.
Here is the pseudocode I am trying but its failing to give me the proper pitch/yaw.
diff = camera_position - enemy_position
hypotenuse = sqrt(diff.x*diff.x + diff.y*diff.y)
angle.x = asinf(diff.z / hypotenuse) * (180 / PI);
angle.y = atan2(diff.y / diff.x) * (180 / PI);
angle.z = 0.0f;
Can someone help me with this? I am terrible at math.
I'm trying to get the angles between two vectors (My Camera Position
and Enemy Position)
In Unity:
Use the Angle function from Vector3 structure.
float angle = Vector3.Angle(camera_position, enemy_position);
Or Individual angles:
float angleX = Vector3.Angle(new Vector3(camera_position.x, 0, 0), new Vector3(enemy_position.x, 0, 0));
float angleY = Vector3.Angle(new Vector3(0, camera_position.y, 0), new Vector3(0, enemy_position.y, 0));
float angleZ = Vector3.Angle(new Vector3(0, 0, camera_position.z), new Vector3(0, 0, enemy_position.z));
EDIT:
I'm not using the Unity engine. This is a separate module I am
creating to rig my own autoaim. I'm trying to do get the proper math
itself.
In C++:
The code is explained in the Angle function below which is the last function
#include <iostream>
#include <numeric> //for inner_product
#include <vector> //For vector
#include <math.h> //For sqrt, acos and M_PI
float Dot(std::vector<float> lhs, std::vector<float> rhs);
float magnitude(std::vector<float> vec3);
float Angle(std::vector<float> from, std::vector<float> to);
std::vector<float> normalise();
int main()
{
std::vector<float> from{3, 1, -2};
std::vector<float> to{5, -3, -7 };
float angle = Angle(from,to);
std::cout<<"Angle: "<<angle<<std::endl;
return 0;
}
//Find Dot/ Scalar product
float Dot(std::vector<float> lhs, std::vector<float> rhs){
return std::inner_product(lhs.begin(), lhs.end(), rhs.begin(), 0);
}
//Find the magnitude of the Vector
float magnitude(std::vector<float> vec3)//<! Vector magnitude
{
return sqrt((vec3[0] * vec3[0]) + (vec3[1] * vec3[1]) + (vec3[2] * vec3[2]));
}
//Normalize Vector. Not needed here
std::vector<float> normalise(std::vector<float> vect)
{
std::vector<float> temp{0, 0, 0};
float length = magnitude(vect);
temp[0] = vect[0]/length;
temp[1] = vect[1]/length;
temp[2] = vect[2]/length;
return temp;
}
float Angle(std::vector<float> from, std::vector<float> to){
//Find the scalar/dot product of the provided 2 Vectors
float dotProduct = Dot(from, to);
//Find the product of both magnitudes of the vectors then divide dot from it
dotProduct = dotProduct / (magnitude(from) * magnitude(to));
//Get the arc cosin of the angle, you now have your angle in radians
float arcAcos = acos(dotProduct);
//Convert to degrees by Multiplying the arc cosin by 180/M_PI
float angle = arcAcos * 180 / M_PI;
return angle;
}
To calculate the angle between two 3d coordinates, in degrees you can use this CalcAngle Function:
#include <algorithm>
#define PI 3.1415927f
struct vec3
{
float x, y, z;
}
vec3 Subtract(vec3 src, vec3 dst)
{
vec3 diff;
diff.x = src.x - dst.x;
diff.y = src.y - dst.y;
diff.z = src.z - dst.z;
return diff;
}
float Magnitude(vec3 vec)
{
return sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z);
}
float Distance(vec3 src, vec3 dst)
{
vec3 diff = Subtract(src, dst);
return Magnitude(diff);
}
vec3 CalcAngle(vec3 src, vec3 dst)
{
vec3 angle;
angle.x = -atan2f(dst.x - src.x, dst.y - src.y) / PI * 180.0f + 180.0f;
angle.y = asinf((dst.z - src.z) / Distance(src, dst)) * 180.0f / PI;
angle.z = 0.0f;
return angle;
}
Complications:
Not all games use the same technique for angles and positions. Min and Max values for x, y and z angles can be different in every game. The basic idea is the same in all games, they just require minor modification to match each game. For example, in the game the code was written for, the X value has to be made negative at the end for it to work.
Another complication is X, Y and Z don't always represent the same variables in both coordinates and angle vec3s.

How to convert mouse coordinate on screen to 3D coordinate

I'm creating a 3D application using GLUT in C++.
Now, I want to implement a method similar to this:
Vector3* MyClass::get3DObjectfromMouse(int mouseX, int mouseY);
How can I implement this method?
As it was commented by Andon M. Coleman, one way you can achieve this is by doing a ray/object intersection test, with unprojected screen coordinates. This technique is commonly known as picking.
A pseudo-C++ code for picking:
Assume we have a 3D object type/class:
class Object3D { ... };
A 3D picking function would return a list of all objects that are intersected by a line going from the given 2D point in the near plane to the same point in the far plane.
struct LineSegment
{
Vector3 start;
Vector3 end;
};
Object3D[] Pick(float x, float y)
{
LineSegment lineSeg;
Object3D[] intersectedObjs;
// Do both un-projections for z-near (0) and z-far (1).
// This produces a line segment going from z-near to far.
UnProject(x, y, /* z = */ 0.0, modelViewMatrix, projectionMatrix, viewport, lineSeg.start);
UnProject(x, y, /* z = */ 1.0, modelViewMatrix, projectionMatrix, viewport, lineSeg.end);
// Iterate all object in the scene or in the current view:
for (Object3D obj : scene)
{
if (TestLineIntersection(obj, lineSeg))
{
// This object is crossed by the picking line.
intersectedObjs.Add(obj);
}
}
// Optionally you might want sort them from distance
// to the camera/viewer before returning the intersections.
return intersectedObjs;
}
And the UnProject() function would look like this:
bool UnProject(float winX, float winY, float winZ,
const Matrix4 & modelView, const Matrix4 & projection,
const ScreenRect viewport, Vector3 & worldCoordinates)
{
// Compute (projection x modelView) ^ -1:
const Matrix4 m = inverse(projection * modelView);
// Need to invert Y since screen Y-origin point down,
// while 3D Y-origin points up (this is an OpenGL only requirement):
winY = viewport.Height() - winY;
// Transformation of normalized coordinates between -1 and 1:
Vector4 in;
in[0] = (winX - viewport.X()) / viewport.Width() * 2.0 - 1.0;
in[1] = (winY - viewport.Y()) / viewport.Height() * 2.0 - 1.0;
in[2] = 2.0 * winZ - 1.0;
in[3] = 1.0;
// To world coordinates:
Vector4 out(m * in);
if (out[3] == 0.0) // Avoid a division by zero
{
worldCoordinates = Vector3Zero;
return false;
}
out[3] = 1.0 / out[3];
worldCoordinates[0] = out[0] * out[3];
worldCoordinates[1] = out[1] * out[3];
worldCoordinates[2] = out[2] * out[3];
return true;
}
To clarify, TestLineIntersection() does a line vs AABB intersection test. The bounding box should be transformed to world-space, since it is usually expressed as a set of points in local model-space.
bool TestLineIntersection(const Object3D & obj, const LineSegment & lineSeg)
{
AABB aabb = obj.GetAABB();
aabb.TransformBy(obj.modelMatrix);
return aabb.LineIntersection(lineSeg.start, lineSeg.end);
}
// AABB.cpp:
bool AABB::LineIntersection(const Vector3 & start, const Vector3 & end) const
{
const Vector3 center = (mins + maxs) * 0.5;
const Vector3 extents = maxs - center;
const Vector3 lineDir = 0.5 * (end - start);
const Vector3 lineCenter = start + lineDir;
const Vector3 dir = lineCenter - center;
const float ld0 = Mathf::Abs(lineDir[0]);
if (Mathf::Abs(dir[0]) > (extents[0] + ld0))
{
return false;
}
const float ld1 = Mathf::Abs(lineDir[1]);
if (Mathf::Abs(dir[1]) > (extents[1] + ld1))
{
return false;
}
const float ld2 = Mathf::Abs(lineDir[2]);
if (Mathf::Abs(dir[2]) > (extents[2] + ld2))
{
return false;
}
const Vector3 vCross = cross(lineDir, dir);
if (Mathf::Abs(vCross[0]) > (extents[1] * ld2 + extents[2] * ld1))
{
return false;
}
if (Mathf::Abs(vCross[1]) > (extents[0] * ld2 + extents[2] * ld0))
{
return false;
}
if (Mathf::Abs(vCross[2]) > (extents[0] * ld1 + extents[1] * ld0))
{
return false;
}
return true;
}

Sphere Collision Algorithm-Overal Velocities Keep increacing (C++, OpenGL)

Im having trouble with my "Box Full of Sphere Particles" project. ive created a spheres[number_of_spheres][position,radius,velocity..] array. A bounding box (sides parallel to orthonormal axis).
Spheres that collide with the box have their velocity reversed on the dimention of the collision. that works ok.
For the sphere-to-sphere collision its pretty simple, i detect the collision by comparing distance to sum of radii. Now im trying to make them simply exchange velocities on the axis of collision. But the simulation runs fine for a while then the speeds keep increasing till its out of control.
bool CollisionDetect(int i, int j)
{
if(i==j)
{
return false;
}
float xx = (spheres[i][0]-spheres[j][0])*(spheres[i][0]-spheres[j][0]);
float yy = (spheres[i][1]-spheres[j][1])*(spheres[i][1]-spheres[j][1]);
float zz = (spheres[i][2]-spheres[j][2])*(spheres[i][2]-spheres[j][2]);
if( (xx + yy + zz) <= (spheres[i][3] + spheres[j][3])*(spheres[i][3] + spheres[j][3]) )
{
return true;
}
else
{
return false;
}
}
void CollisionSolve(int i, int j)
{
float m1,m2,m21;
Vec3 vel1,vel2,pos1,pos2; //spheres info
Vec3 nv1n,nv1b,nv1t;
Vec3 nv2n,nv2b,nv2t;
Vec3 N,T,B,X,Y,Z; //collision plane and world orthonormal basis
X.x=1;
X.y=0;
X.z=0;
Y.x=0;
Y.y=1;
Y.z=0;
Z.x=0;
Z.y=0;
Z.z=1;
pos1.x = spheres[i][0];
pos2.x = spheres[j][0];
pos1.y = spheres[i][1];
pos2.y = spheres[j][1];
pos1.z = spheres[i][2];
pos2.z = spheres[j][2];
vel1.x = spheres[i][4];
vel2.x = spheres[j][4];
vel1.y = spheres[i][5];
vel2.y = spheres[j][5];
vel1.z = spheres[i][6];
vel2.z = spheres[j][6];
m1 = spheres[i][8];
m2 = spheres[j][8]; //mass (for later)
N = minus(pos2,pos1); //get N vector (connecting centers of spheres)
N = Normalize(N);
T=X;
B = crossProduct(N,T); //find first perpendicular axis to N
if (B.x==0 && B.y==0 && B.z==0) //then vector parallel to X axis -
{
T=Y; //try Y axis
B = crossProduct(N,T);
}
T = crossProduct(N,B); //find second perpendicular axis to N
T = Normalize(T);
B = Normalize(B);
if (simplespherecollision)
{
nv1n = projectUonV (vel1 , N);
nv2n = projectUonV (vel2 , N);
vel1 = minus (vel1,nv1n);
vel2 = minus (vel2,nv2n);
vel1 = plus (vel1,nv2n);
vel2 = plus (vel2,nv1n);
//simply switch speed (for test)
}
/*/---THIS IS COMMENTED OUT (FIRST METHOD USED - DIDNT WORK)--------------------------------------------
nv1n = projectUonV (vel1 , N);
nv2n = projectUonV (vel2 , N);
nv1t = projectUonV (vel1 , T);
nv2t = projectUonV (vel2 , T);
nv1b = projectUonV (vel1 , B);
nv2b = projectUonV (vel2 , B); //project velocities on new orthonormal basis
vel1 = plus(nv1t , plus(nv1b , nv2n)); //project velocities back to world basis
vel2 = plus(nv2t , plus( nv2b , nv1n)); //by adding the sub vectors with swiched Xn
/*/----------------------------------------------------------------------
spheres[i][4] = vel1.x;
spheres[i][5] = vel1.y;
spheres[i][6] = vel1.z;
spheres[j][4] = vel2.x;
spheres[j][5] = vel2.y;
spheres[j][6] = vel2.z; //reasign velocities to spheres
}
}
And this is the Vec3 struct (just in case)
struct Vec3
{
float x, y, z;
};
Vec3 crossProduct(const Vec3& v1, const Vec3& v2)
{
Vec3 r;
r.x = (v1.y*v2.z) - (v1.z*v2.y);
r.y = (v1.z*v2.x) - (v1.x*v2.z);
r.z = (v1.x*v2.y) - (v1.y*v2.x);
return r;
}
Vec3 minus(const Vec3& v1, const Vec3& v2)
{
Vec3 r;
r.x = v1.x - v2.x;
r.y = v1.y - v2.y;
r.z = v1.z - v2.z;
return r;
}
Vec3 plus(const Vec3& v1, const Vec3& v2)
{
Vec3 r;
r.x = v1.x + v2.x;
r.y = v1.y + v2.y;
r.z = v1.z + v2.z;
return r;
}
double dotProduct(const Vec3& v1, const Vec3& v2)
{
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
Vec3 scale(const Vec3& v, double a)
{
Vec3 r;
r.x = v.x * a;
r.y = v.y * a;
r.z = v.z * a;
return r;
}
Vec3 projectUonV(const Vec3& u, const Vec3& v)
{
Vec3 r;
r = scale(v,dotProduct(u,v));
return r;
}
int distanceSquared(const Vec3& v1, const Vec3& v2)
{
Vec3 delta = minus(v2, v1);
return dotProduct(delta, delta);
}
Vec3 Normalize (const Vec3& v)
{
Vec3 r;
r=v;
int mag = sqrt(dotProduct(v,v));
r.x = v.x/mag;
r.y = v.y/mag;
r.z = v.z/mag;
return r;
}
And this is my Render function witch draws the spheres and the bounding box and calls the other functions
void Render()
{
//CLEARS FRAME BUFFER ie COLOR BUFFER& DEPTH BUFFER (1.0)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clean up the colour of the window
// and the depth buffer
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glTranslatef(-MAX_X/2,-MAX_Y/2,-MAX_Z*4);
glTranslatef (dx,0,dz);
//------------------------------------------------------------------------------------------------
//-----------------------------BOUNDING BOX----------------------------------------------------
glPushMatrix();
glTranslatef(MAX_X/2,MAX_Y/2,MAX_Z/2);
glColor4f(0.9,0.6,0.8,0.3);
glScalef(MAX_X,MAX_Y,MAX_Z);
glutSolidCube(1);
glutWireCube(1);
glPopMatrix();
//-----------------------------WALL COLLISION DETECTION-----ALWAYS ON--------------------------------
for (int i = 0; i<PART_NUM; i++)
{
if(spheres[i][0] > (MAX_X - spheres[i][3]) && spheres[i][4] > 0)
{
spheres[i][4] = spheres[i][4]*-1;
wallcollcount++;
}
if(spheres[i][1] > (MAX_Y - spheres[i][3]) && spheres[i][5] > 0)
{
spheres[i][5] = spheres[i][5]*-1;
wallcollcount++;
}
if(spheres[i][2] > (MAX_Z - spheres[i][3]) && spheres[i][6] > 0)
{
spheres[i][6] = spheres[i][6]*-1;
wallcollcount++;
}
if(spheres[i][0] < spheres[i][3] && spheres[i][4] < 0)
{
spheres[i][4] = spheres[i][4]*-1;
wallcollcount++;
}
if(spheres[i][1] < spheres[i][3] && spheres[i][5] < 0)
{
spheres[i][5] = spheres[i][5]*-1;
wallcollcount++;
}
if(spheres[i][2] < spheres[i][3] && spheres[i][6] < 0)
{
spheres[i][6] = spheres[i][6]*-1;
wallcollcount++;
}
//--------------------------------------Sphere ColDit -------------------------
if (spherecollision || simplespherecollision)
{
for(int j=i+1; j<PART_NUM; j++)
{
if(CollisionDetect(i,j))
{
spherecollcount++;
CollisionSolve(i,j);
}
}
}
//-----------------------------------------------DRAW--------------------------------
glPushMatrix();
glTranslatef(spheres[i][0],spheres[i][1],spheres[i][2]);
glColor3f( (float) (i+1)/(PART_NUM) , 1-(float)(i+1)/(PART_NUM) , 0.5);
glutSolidSphere(spheres[i][3], 18,18);
glPopMatrix();
}
//---------------------------------------------------------------------------------------------------------------/
glutSwapBuffers(); // All drawing commands applied to the
// hidden buffer, so now, bring forward
// the hidden buffer and hide the visible one
}
There are two things conserved in a collision:
momentum
and
energy
So far your calculations just consider the momentum (a simply speed swap assumes a so called central collision and identical masses of the spheres). If computers were infinitely precise this would work out perfectly. But computers have only limited precisions and so roundoff errors will creep in.
The simple solution for that problem is to correct the momentum part (which is mass · velocity) for the deviation in energy (1/2 mass · velocity²). This works because energy depends by the square of the velocity so small deviations on the speeds involved will create large energy deviations which you can use for correcting the calculation.
I.e. you calculate the total energy before the collision and then after the collision. Then you take the ratio sqrt(E_before / E_after) and scale the speeds after the calculation with that.
If you want to be really accurate you could do relativistic momentum and energy transfer ;)

Build Circle from 3 Points in 3D space implementation in C or C++

We have 3(three) xyz points that define a circle in 3D space, this circle needs to be converted into a polyline(for further rendering). I'm looking for a ready C or C++ function or free library that can do the job.
Don't understand why this was closed. And I can't even answer my own question there. Shame on you guys. But you will not stop the knowledge spreading!
There's a much simpler solution to find the circle parameters in real 3D, just take a look at the "barycentric coordinates" section in http://en.wikipedia.org/wiki/Circumscribed_circle .
You can extract the following optimized code from that:
// triangle "edges"
const Vector3d t = p2-p1;
const Vector3d u = p3-p1;
const Vector3d v = p3-p2;
// triangle normal
const Vector3d w = t.crossProduct(u);
const double wsl = w.getSqrLength();
if (wsl<10e-14) return false; // area of the triangle is too small (you may additionally check the points for colinearity if you are paranoid)
// helpers
const double iwsl2 = 1.0 / (2.0*wsl);
const double tt = t*t;
const double uu = u*u;
// result circle
Vector3d circCenter = p1 + (u*tt*(u*v) - t*uu*(t*v)) * iwsl2;
double circRadius = sqrt(tt * uu * (v*v) * iwsl2*0.5);
Vector3d circAxis = w / sqrt(wsl);
You can then calculate the points on the circle in real 3D too and e.g. draw them using GL_LINE_STRIP in OpenGL. This should be much faster than using the 2D sin/cos approach.
// find orthogonal vector to the circle axis
const Vector3d an = circAxis.getNormalized();
const Vector3d ao = Vector3d(4.0+an[0], 4.0+an[0]+an[1], 4.0+an[0]+an[1]+an[2]).crossProduct(an).getNormalized();
// 4x4 rotation matrix around the circle axis
const int steps = 360; // maybe adjust according to circle size on screen
Matrix4d R = makeRotMatrix4d(circCenter, circAxis, 2.0*M_PI/double(steps));
// one point on the circle
Vector3d cp = circCenter + ao*circRadius;
// rotate point on the circle
for (int i=0; i<steps; ++i)
{
circlePoints.push_back(cp);
cp = transformPoint(cp, R); // apply the matrix
}
For the creation of the transformation matrix (i.e. makeRotMatrix4d()) see http://paulbourke.net/geometry/rotate/ for example.
Please note that I did not test if the above code really compiles, but it should give you enough hints.
There is a nice article and a code sample on how to build a circle by 3 points in 2D, XY plane.
http://paulbourke.net/geometry/circlesphere/
http://paulbourke.net/geometry/circlesphere/Circle.cpp
To build a 3D circle we'll have to:
rotate our 3 points into XY plane
Calculate circle center
build a circle in XY plane using the code in the article
rotate it back into it's original plane
For rotations it is best to use quaternions.
To find a correct quaternion I looked at Ogre3d source code:
void Quaternion::FromAngleAxis (const Radian& rfAngle, const Vector3& rkAxis)
There is one more useful function there:
Quaternion getRotationTo(const Vector3& dest, const Vector3& fallbackAxis = Vector3::ZERO) const
But I didn't use it.
For quaterions and vectors I used our own classes. Here is the full source code of the function that does the job:
bool IsPerpendicular(Point3d *pt1, Point3d *pt2, Point3d *pt3);
double CalcCircleCenter(Point3d *pt1, Point3d *pt2, Point3d *pt3, Point3d *center);
void FindCircleCenter(const Point3d *V1, const Point3d *V2, const Point3d *V3, Point3d *center)
{
Point3d *pt1=new Point3d(*V1);
Point3d *pt2=new Point3d(*V2);
Point3d *pt3=new Point3d(*V3);
if (!IsPerpendicular(pt1, pt2, pt3) ) CalcCircleCenter(pt1, pt2, pt3, center);
else if (!IsPerpendicular(pt1, pt3, pt2) ) CalcCircleCenter(pt1, pt3, pt2, center);
else if (!IsPerpendicular(pt2, pt1, pt3) ) CalcCircleCenter(pt2, pt1, pt3, center);
else if (!IsPerpendicular(pt2, pt3, pt1) ) CalcCircleCenter(pt2, pt3, pt1, center);
else if (!IsPerpendicular(pt3, pt2, pt1) ) CalcCircleCenter(pt3, pt2, pt1, center);
else if (!IsPerpendicular(pt3, pt1, pt2) ) CalcCircleCenter(pt3, pt1, pt2, center);
else {
delete pt1;
delete pt2;
delete pt3;
return;
}
delete pt1;
delete pt2;
delete pt3;
}
bool IsPerpendicular(Point3d *pt1, Point3d *pt2, Point3d *pt3)
// Check the given point are perpendicular to x or y axis
{
double yDelta_a= pt2->y - pt1->y;
double xDelta_a= pt2->x - pt1->x;
double yDelta_b= pt3->y - pt2->y;
double xDelta_b= pt3->x - pt2->x;
// checking whether the line of the two pts are vertical
if (fabs(xDelta_a) <= 0.000000001 && fabs(yDelta_b) <= 0.000000001){
return false;
}
if (fabs(yDelta_a) <= 0.0000001){
return true;
}
else if (fabs(yDelta_b) <= 0.0000001){
return true;
}
else if (fabs(xDelta_a)<= 0.000000001){
return true;
}
else if (fabs(xDelta_b)<= 0.000000001){
return true;
}
else
return false ;
}
double CalcCircleCenter(Point3d *pt1, Point3d *pt2, Point3d *pt3, Point3d *center)
{
double yDelta_a = pt2->y - pt1->y;
double xDelta_a = pt2->x - pt1->x;
double yDelta_b = pt3->y - pt2->y;
double xDelta_b = pt3->x - pt2->x;
if (fabs(xDelta_a) <= 0.000000001 && fabs(yDelta_b) <= 0.000000001){
center->x= 0.5*(pt2->x + pt3->x);
center->y= 0.5*(pt1->y + pt2->y);
center->z= pt1->z;
return 1;
}
// IsPerpendicular() assure that xDelta(s) are not zero
double aSlope=yDelta_a/xDelta_a; //
double bSlope=yDelta_b/xDelta_b;
if (fabs(aSlope-bSlope) <= 0.000000001){ // checking whether the given points are colinear.
return -1;
}
// calc center
center->x= (aSlope*bSlope*(pt1->y - pt3->y) + bSlope*(pt1->x + pt2 ->x)
- aSlope*(pt2->x+pt3->x) )/(2* (bSlope-aSlope) );
center->y = -1*(center->x - (pt1->x+pt2->x)/2)/aSlope + (pt1->y+pt2->y)/2;
return 1;
}
//! Builds a circle in 3D space by 3 points on it and an optional center
void buildCircleBy3Pt(const float *pt1,
const float *pt2,
const float *pt3,
const float *c, // center, can be NULL
std::vector<float> *circle)
{
/* Get the normal vector to the triangle formed by 3 points
Calc a rotation quaternion from that normal to the 0,0,1 axis
Rotate 3 points using quaternion. Points will be in XY plane
Build a circle by 3 points on XY plane
Rotate a circle back into original plane using quaternion
*/
Point3d p1(pt1[0], pt1[1], pt1[2]);
Point3d p2(pt2[0], pt2[1], pt2[2]);
Point3d p3(pt3[0], pt3[1], pt3[2]);
Point3d center;
if (c)
{
center.set(c[0], c[1], c[2]);
}
const Vector3d p2top1 = p1 - p2;
const Vector3d p2top3 = p3 - p2;
const Vector3d circle_normal = p2top1.crossProduct(p2top3).normalize();
const Vector3d xy_normal(0, 0, 1);
Quaternion rot_quat;
// building rotation quaternion
{
// Rotation axis around which we will rotate our circle into XY plane
Vector3d rot_axis = xy_normal.crossProduct(circle_normal).normalize();
const double rot_angle = xy_normal.angleTo(circle_normal); // radians
const double w = cos(rot_angle * 0.5);
rot_axis *= sin(rot_angle * 0.5);
rot_quat.set(w, rot_axis.x, rot_axis.y, rot_axis.z);
}
Quaternion rot_back_quat;
// building backward rotation quaternion, same as prev. but -angle
{
const double rot_angle = -(xy_normal.angleTo(circle_normal)); // radians
const double w_back = cos(rot_angle * 0.5);
Vector3d rot_back_axis = xy_normal.crossProduct(circle_normal).normalize();
rot_back_axis *= sin(rot_angle * 0.5);
rot_back_quat.set(w_back, rot_back_axis.x, rot_back_axis.y, rot_back_axis.z);
}
rot_quat.rotate(p1);
rot_quat.rotate(p2);
rot_quat.rotate(p3);
rot_quat.rotate(center);
if (!c)
{
// calculate 2D center
FindCircleCenter(&p1, &p2, &p3, &center);
}
// calc radius
const double radius = center.distanceTo(p1);
const float DEG2RAD = 3.14159f / 180.0f;
// build circle
for (int i = 0; i < 360; ++i)
{
float degInRad = i * DEG2RAD;
Point3d pt(cos(degInRad) * radius + center.x, sin(degInRad) * radius + center.y, 0);
// rotate the point back into original plane
rot_back_quat.rotate(pt);
circle->push_back(pt.x);
circle->push_back(pt.y);
circle->push_back(pt.z);
}
}
The following is the C#/Unity port of Mark's answer. It uses types and utility functions from Unity's scripting API.
// triangle "edges"
var t = p2 - p1;
var u = p3 - p1;
var v = p3 - p2;
// triangle normal
var w = Vector3.Cross(t, u);
var wsl = Vector3.Dot(w, w);
// TODO: if (wsl<10e-14) return false; // area of the triangle is too small (you may additionally check the points for colinearity if you are paranoid)
// helpers
var iwsl2 = 1f / (2f * wsl);
var tt = Vector3.Dot(t, t);
var uu = Vector3.Dot(u, u);
// result circle
Vector3 circCenter = p1 + (u * tt * (Vector3.Dot(u, v)) - t * uu * (Vector3.Dot(t, v))) * iwsl2;
var circRadius = Mathf.Sqrt(tt * uu * (Vector3.Dot(v, v)) * iwsl2 * 0.5f);
Vector3 circAxis = w / Mathf.Sqrt(wsl);
Using Unity's Gizmos, the circle can be drawn as follows (using 30 line segments to approximate it in this case):
// Draw the circle:
Gizmos.color = Color.white;
for (int i = 0; i < 30; ++i)
{
Gizmos.DrawLine(
circCenter + Quaternion.AngleAxis(360f / 30f * i , circAxis) * (p1 - circCenter),
circCenter + Quaternion.AngleAxis(360f / 30f * (i + 1), circAxis) * (p1 - circCenter)
);
}
The result looks like follows for vertex positions var p1 = new Vector3(0f, 1.44f, 0f); var p2 = new Vector3(0f, 0.73f, 0.65f); var p3 = new Vector3(0f, -1.04f, 0f);: