.
The routines below implement some standard geometry stuff. They work for me. If they don't work for you, that's your problem, not mine. Check that you're living in a 3-dimensional world. Several of them could be faster, if you consider speed more important than readability.
For more information, see a good geometry textbook. I recommend either of:
C.E.Weatherburn, Elementary Vector Analysis, Bell, 1921.
W.H.Macaulay, Solid Geometry, Cambridge University Press, 1930.
Type Definitions
Vector Operations
2D Geometric Operations
3D Geometric Operations
/* ------------------------------------------------------ */
typedef double DEGREES;
typedef double RADIANS;
/* ------------------------------------------------------ */
typedef struct { double x,y; } POINT2D, VECTOR2D;
/* ------------------------------------------------------ */
/* Express the equation of a line as a*x+b*y+c = 0 */
/* The normal vector is then [a,b], and the distance [c] */
typedef struct { VECTOR2D Normal; double Distance; } IMPLICIT_LINE;
/* ------------------------------------------------------ */
typedef struct { double x, y, z; } POINT3D, VECTOR3D;
/* -------------------------------------------------------- */
/* Express the equation of a face as p*x+q*y+r*z+d = 0 */
/* The normal vector is then [p,q,r], and the distance [d] */
typedef struct { VECTOR3D Normal; double Distance; } IMPLICIT_PLANE;
/* ------------------------------------------------------ */
/* -------------------------------------------------------------------------------- */
VECTOR3D *GetVectorFromAtoB (VECTOR3D *V, POINT3D *A, POINT3D *B)
{
V->x = B->x - A->x;
V->y = B->y - A->y;
V->z = B->z - A->z;
return V;
}
/* -------------------------------------------------------------------------------- */
VECTOR3D *GetVectorSum (VECTOR3D *Vector, VECTOR3D *A, VECTOR3D *B)
{
Vector->x = B->x + A->x;
Vector->y = B->y + A->y;
Vector->z = B->z + A->z;
return Vector;
}
/* -------------------------------------------------------------------------------- */
double DotProduct (VECTOR3D *Vector1, VECTOR3D *Vector2)
{
return (Vector1->x*Vector2->x) + (Vector1->y*Vector2->y) + (Vector1->z*Vector2->z);
}
/* -------------------------------------------------------------------------------- */
VECTOR3D *GetCrossProduct (VECTOR3D *CrossProduct, VECTOR3D *A, VECTOR3D *B)
{
CrossProduct->x = (A->y*B->z) - (A->z*B->y);
CrossProduct->y = (A->z*B->x) - (A->x*B->z);
CrossProduct->z = (A->x*B->y) - (A->y*B->x);
return CrossProduct;
}
/* -------------------------------------------------------------------------------- */
double ScalarTripleProduct (VECTOR3D *Vector1, VECTOR3D *Vector2, VECTOR3D *Vector3)
{
VECTOR3D Cross;
GetCrossProduct (&Cross, Vector2, Vector3);
return DotProduct (Vector1, &Cross);
}
/* -------------------------------------------------------------------------------- */
VECTOR3D *NormaliseVector (VECTOR3D *Vector)
{
double X = Vector->x;
double Y = Vector->y;
double Z = Vector->z;
double D = sqrt(X*X+Y*Y+Z*Z);
Vector->x /= D;
Vector->y /= D;
Vector->z /= D;
return Vector;
}
/* -------------------------------------------------------------------------------- */
/* Given vectors V and W, make V' the nearest vector to V perpendicular to W */
void MakeVPerpendicularToW (VECTOR3D *V, VECTOR3D *W)
{
double VxWx = V->x * W->x;
double VyWy = V->y * W->y;
double VzWz = V->z * W->z;
double WxWx = W->x * W->x;
double WyWy = W->y * W->y;
double WzWz = W->z * W->z;
double Vx = V->x*(WyWy+WzWz) - W->x*(VyWy+VzWz);
double Vy = V->y*(WzWz+WxWx) - W->y*(VzWz+VxWx);
double Vz = V->z*(WxWx+WyWy) - W->z*(VxWx+VyWy);
V->x = Vx;
V->y = Vy;
V->z = Vz;
}
/* -------------------------------------------------------------------------------- */
/* Given a vector V and an axis A, this gets a vector R which has the properties: */
/* R.A = V.A; R.(VxA) = 1 */
/* It assumes that V and A are pre-normalised */
void GetPerpendicularAtSameAngle (VECTOR3D *Result, VECTOR3D *Axis, VECTOR3D *Vector)
{
VECTOR3D Cross;
double CosAngle,SinAngle;
CosAngle = DotProduct(Vector,Axis);
SinAngle = sqrt(1.0-CosAngle*CosAngle);
GetCrossProduct(&Cross,Vector,Axis);
Result->x = Axis->x * CosAngle + Cross.x * SinAngle;
Result->y = Axis->y * CosAngle + Cross.y * SinAngle;
Result->z = Axis->z * CosAngle + Cross.z * SinAngle;
}
/* -------------------------------------------------------------------------------- */
/* The following routines take three non-coplanar vectors and turn them into three */
/* mutually-perpendicular vectors to within one part in 10^10 or thereabouts */
static void IterativelyImproveNormals (VECTOR3D *V1, VECTOR3D *V2, VECTOR3D *V3)
{
VECTOR3D NewGuesses[3];
if (DotProduct(V1,GetCrossProduct(&NewGuesses[0],V2,V3)) < 0) NegateVector(&NewGuesses[0]);
if (DotProduct(V2,GetCrossProduct(&NewGuesses[1],V1,V3)) < 0) NegateVector(&NewGuesses[1]);
if (DotProduct(V3,GetCrossProduct(&NewGuesses[2],V1,V2)) < 0) NegateVector(&NewGuesses[2]);
NormaliseVector(AddVector(V1,NormaliseVector(&NewGuesses[0])));
NormaliseVector(AddVector(V2,NormaliseVector(&NewGuesses[1])));
NormaliseVector(AddVector(V3,NormaliseVector(&NewGuesses[2])));
}
/* -------------------------------------------------------------------------------- */
void MakeVectorsOrthogonal (VECTOR3D *V1, VECTOR3D *V2, VECTOR3D *V3)
{
IterativelyImproveNormals (V1,V2,V3);
IterativelyImproveNormals (V1,V2,V3);
IterativelyImproveNormals (V1,V2,V3);
IterativelyImproveNormals (V1,V2,V3);
}
/* -------------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------------- */
void NormaliseImplicitLine (IMPLICIT_LINE *G)
{
double A = G->Normal.x;
double B = G->Normal.y;
double C = G->Distance;
double D = sqrt(A*A+B*B);
if (C < 0.0) D = -D;
G->Normal.x = A/D;
G->Normal.y = B/D;
G->Distance = C/D;
}
/* -------------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------------- */
void NormaliseImplicitPlane (IMPLICIT_PLANE *Plane)
{
double P = Plane->Normal.x;
double Q = Plane->Normal.y;
double R = Plane->Normal.z;
double C = Plane->Distance;
double D = sqrt(P*P+Q*Q+R*R);
if (C < 0.0) D = -D;
Plane->Normal.x = P/D;
Plane->Normal.y = Q/D;
Plane->Normal.z = R/D;
Plane->Distance = C/D;
}
/* -------------------------------------------------------------------------------- */
/* Find Implicit Plane through three points, */
/* assumes that the points are non-collinear. */
void FindImplicitPlane (IMPLICIT_PLANE *Plane, POINT3D *Point1, POINT3D *Point2, POINT3D *Point3)
{
VECTOR3D V12, V13;
GetVectorFromAtoB(&V12,Point1,Point2);
GetVectorFromAtoB(&V13,Point1,Point3);
GetCrossProduct(&Plane->Normal,&V12,&V13);
Plane->Distance = - ((Point1->x*Plane->Normal.x) + (Point1->y*Plane->Normal.y) + (Point1->z*Plane->Normal.z));
NormaliseImplicitPlane(Plane);
}
/* -------------------------------------------------------------------------------- */
/* Reflect point across general implicit plane (assumes plane normalised) */
void ReflectPointThroughPlane (POINT3D *Point, IMPLICIT_PLANE *Plane)
{
double U = - ((Point->x*Plane->Normal.x) + (Point->y*Plane->Normal.y) + (Point->z*Plane->Normal.z) + Plane->Distance);
Point->x += 2.0 * U * Plane->Normal.x;
Point->y += 2.0 * U * Plane->Normal.y;
Point->z += 2.0 * U * Plane->Normal.z;
}
/* -------------------------------------------------------------------------------- */
/* Moving a point into a plane is almost exactly the same as reflecting a point */
/* through a plane, the only difference being that we only go half-way along the */
/* line. Again, it is assumed that the plane is normalised. */
void MovePointToPlane (POINT3D *Point, IMPLICIT_PLANE *Plane)
{
double U;
U = - ((Point->x*Plane->Normal.x) + (Point->y*Plane->Normal.y) + (Point->z*Plane->Normal.z) + Plane->Distance);
Point->x += U * Plane->Normal.x;
Point->y += U * Plane->Normal.y;
Point->z += U * Plane->Normal.z;
}
/* -------------------------------------------------------------------------------- */
/* Reflect a normalised vector starting at the origin across a normalised axis also */
/* starting at the origin. */
void ReflectVectorAcrossAxis (VECTOR3D *Vector, VECTOR3D *Axis)
{
double U = DotProduct(Vector,Axis);
Vector->x = 2.0 * U * Axis->x - Vector->z;
Vector->y = 2.0 * U * Axis->y - Vector->z;
Vector->z = 2.0 * U * Axis->z - Vector->z;
}
/* -------------------------------------------------------------------------------- */
/* Reflect a normalised vector starting at the origin through a mirror plane also */
/* through the origin and defined by a normalised normal. */
void ReflectVectorThroughMirror (VECTOR3D *Vector, VECTOR3D *MirrorNormal)
{
double U = DotProduct(Vector,MirrorNormal);
Vector->x -= 2.0 * U * MirrorNormal->x;
Vector->y -= 2.0 * U * MirrorNormal->y;
Vector->z -= 2.0 * U * MirrorNormal->z;
}
/* -------------------------------------------------------------------------------- */
/* Rotate any point about any normalised axis starting at the origin */
void RotatePointAboutAxis (POINT3D *Point, VECTOR3D *Axis, DEGREES Angle)
{
double sinAngle = sin(Angle*M_PI/180.0);
double cosAngle = cos(Angle*M_PI/180.0);
double verAngle = 1-cosAngle;
double x = Point->x;
double y = Point->y;
double z = Point->z;
double p = Axis->x;
double q = Axis->y;
double r = Axis->z;
double pp = p*p;
double qq = q*q;
double rr = r*r;
double pqv = p*q*verAngle;
double prv = p*r*verAngle;
double qrv = q*r*verAngle;
Point->x = x*(pp+(qq+rr)*cosAngle) + y*(pqv-r*sinAngle) + z*(prv+q*sinAngle);
Point->y = x*(pqv+r*sinAngle) + y*(qq+(pp+rr)*cosAngle) + z*(qrv-p*sinAngle);
Point->z = x*(prv-q*sinAngle) + y*(qrv+p*sinAngle) + z*(rr+(pp+qq)*cosAngle);
}
/* -------------------------------------------------------------------------------- */
/* Rotate any point about any normalised axis starting anywhere */
void RotatePointAboutOffsetAxis (POINT3D *Point, VECTOR3D *Axis, POINT3D *AxisOffset, DEGREES Angle)
{
Point->x -= AxisOffset->x;
Point->y -= AxisOffset->y;
Point->z -= AxisOffset->z;
RotatePointAboutAxis (Point, Axis, Angle);
Point->x += AxisOffset->x;
Point->y += AxisOffset->y;
Point->z += AxisOffset->z;
}
/* -------------------------------------------------------------------------------- */