diff options
Diffstat (limited to 'src/Quaternions.cpp')
| -rw-r--r-- | src/Quaternions.cpp | 148 |
1 files changed, 71 insertions, 77 deletions
diff --git a/src/Quaternions.cpp b/src/Quaternions.cpp index 9ce344f..90c7502 100644 --- a/src/Quaternions.cpp +++ b/src/Quaternions.cpp @@ -113,10 +113,10 @@ quaternion To_Quat(Matrix_t m) { // From Jason Shankel, (C) 2000. quaternion Quat; - + double Tr = m[0][0] + m[1][1] + m[2][2] + 1.0, fourD; double q[4]; - + int i,j,k; if (Tr >= 1.0) { @@ -148,7 +148,7 @@ quaternion To_Quat(Matrix_t m) q[k] = (m[k][i] + m[i][k]) / fourD; q[3] = (m[j][k] - m[k][j]) / fourD; } - + Quat.x = q[0]; Quat.y = q[1]; Quat.z = q[2]; @@ -186,7 +186,7 @@ quaternion To_Quat(angle_axis Ang_Ax) { // From the Quaternion Powers article on gamedev.net quaternion Quat; - + Quat.x = Ang_Ax.x * sin(Ang_Ax.angle / 2); Quat.y = Ang_Ax.y * sin(Ang_Ax.angle / 2); Quat.z = Ang_Ax.z * sin(Ang_Ax.angle / 2); @@ -202,7 +202,7 @@ angle_axis Quat_2_AA(quaternion Quat) Ang_Ax.x = Quat.x / scale; Ang_Ax.y = Quat.y / scale; Ang_Ax.z = Quat.z / scale; - + Ang_Ax.angle = 2.0 * acos(Quat.w)/(float)PI*180; return Ang_Ax; } @@ -226,23 +226,23 @@ quaternion To_Quat(int In_Degrees, euler Euler) sr = float(sin(Euler.x/2)); sp = float(sin(Euler.y/2)); sy = float(sin(Euler.z/2)); - + cpcy = cp * cy; spsy = sp * sy; Quat.w = cr * cpcy + sr * spsy; Quat.x = sr * cpcy - cr * spsy; Quat.y = cr * sp * cy + sr * cp * sy; Quat.z = cr * cp * sy - sr * sp * cy; - + return Quat; } - + quaternion QNormalize(quaternion Quat) { float norm; - norm = Quat.x * Quat.x + - Quat.y * Quat.y + - Quat.z * Quat.z + + norm = Quat.x * Quat.x + + Quat.y * Quat.y + + Quat.z * Quat.z + Quat.w * Quat.w; Quat.x = float(Quat.x / norm); Quat.y = float(Quat.y / norm); @@ -259,13 +259,13 @@ XYZ Quat2Vector(quaternion Quat) float fX = Quat.x; float fY = Quat.y; float fZ = Quat.z; - + XYZ tempvec; tempvec.x = 2.0f*(fX*fZ-fW*fY); tempvec.y = 2.0f*(fY*fZ+fW*fX); tempvec.z = 1.0f-2.0f*(fX*fX+fY*fY); - + return tempvec; } @@ -305,37 +305,36 @@ extern float normalv[3]; bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33) { bInter=0; - + pointv[0]=p->x; pointv[1]=p->y; pointv[2]=p->z; - - + p1v[0]=p11; p1v[1]=p12; p1v[2]=p13; - + p2v[0]=p21; p2v[1]=p22; p2v[2]=p23; - + p3v[0]=p31; p3v[1]=p32; p3v[2]=p33; - + normalv[0]=normal.x; normalv[1]=normal.y; normalv[2]=normal.z; - + #define ABS(X) (((X)<0.f)?-(X):(X) ) -#define MAX(A, B) (((A)<(B))?(B):(A)) +#define MAX(A, B) (((A)<(B))?(B):(A)) float max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2])); #undef MAX if (max == ABS(normalv[0])) {i = 1; j = 2;} // y, z if (max == ABS(normalv[1])) {i = 0; j = 2;} // x, z if (max == ABS(normalv[2])) {i = 0; j = 1;} // x, y #undef ABS - + u0 = pointv[i] - p1v[i]; v0 = pointv[j] - p1v[j]; u1 = p2v[i] - p1v[i]; @@ -373,63 +372,62 @@ bool LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector *p) float denom, mu; Vector n, pa1, pa2, pa3; - //Calculate the parameters for the plane + //Calculate the parameters for the plane n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y); n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z); n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x); n.Normalize(); d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; - //Calculate the position on the line that intersects the plane + //Calculate the position on the line that intersects the plane denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); - if (abs(denom) < 0.0000001) // Line and plane don't intersect + if (abs(denom) < 0.0000001) // Line and plane don't intersect return 0; mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; p->x = p1.x + mu * (p2.x - p1.x); p->y = p1.y + mu * (p2.y - p1.y); p->z = p1.z + mu * (p2.z - p1.z); - if (mu < 0 || mu > 1) // Intersection not along line segment + if (mu < 0 || mu > 1) // Intersection not along line segment return 0; - + if(!PointInTriangle( p, n, pa.x, pa.y, pa.z, pb.x, pb.y, pb.z, pc.x, pc.y, pc.z)){return 0;} - + return 1; } bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3) { bInter=0; - + pointv[0]=p->x; pointv[1]=p->y; pointv[2]=p->z; - - + p1v[0]=p1->x; p1v[1]=p1->y; p1v[2]=p1->z; - + p2v[0]=p2->x; p2v[1]=p2->y; p2v[2]=p2->z; - + p3v[0]=p3->x; p3v[1]=p3->y; p3v[2]=p3->z; - + normalv[0]=normal.x; normalv[1]=normal.y; normalv[2]=normal.z; - + #define ABS(X) (((X)<0.f)?-(X):(X) ) -#define MAX(A, B) (((A)<(B))?(B):(A)) +#define MAX(A, B) (((A)<(B))?(B):(A)) float max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2])); #undef MAX if (max == ABS(normalv[0])) {i = 1; j = 2;} // y, z if (max == ABS(normalv[1])) {i = 0; j = 2;} // x, z if (max == ABS(normalv[2])) {i = 0; j = 1;} // x, y #undef ABS - + u0 = pointv[i] - p1v[i]; v0 = pointv[j] - p1v[j]; u1 = p2v[i] - p1v[i]; @@ -467,26 +465,26 @@ bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p) float denom, mu; XYZ n; - //Calculate the parameters for the plane + //Calculate the parameters for the plane n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y); n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z); n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x); Normalise(&n); d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; - //Calculate the position on the line that intersects the plane + //Calculate the position on the line that intersects the plane denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); - if (abs(denom) < 0.0000001) // Line and plane don't intersect + if (abs(denom) < 0.0000001) // Line and plane don't intersect return 0; mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; p->x = p1.x + mu * (p2.x - p1.x); p->y = p1.y + mu * (p2.y - p1.y); p->z = p1.z + mu * (p2.z - p1.z); - if (mu < 0 || mu > 1) // Intersection not along line segment + if (mu < 0 || mu > 1) // Intersection not along line segment return 0; - + if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;} - + return 1; } @@ -497,47 +495,46 @@ extern XYZ pa1,pa2,pa3,n; float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p) { - - //Calculate the parameters for the plane + //Calculate the parameters for the plane n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y); n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z); n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x); Normalise(&n); d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; - //Calculate the position on the line that intersects the plane + //Calculate the position on the line that intersects the plane denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); - if (abs(denom) < 0.0000001) // Line and plane don't intersect + if (abs(denom) < 0.0000001) // Line and plane don't intersect return 0; mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; p->x = p1.x + mu * (p2.x - p1.x); p->y = p1.y + mu * (p2.y - p1.y); p->z = p1.z + mu * (p2.z - p1.z); - if (mu < 0 || mu > 1) // Intersection not along line segment + if (mu < 0 || mu > 1) // Intersection not along line segment return 0; - + if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;} - + return 1; } float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc, XYZ n, XYZ *p) { - //Calculate the parameters for the plane + //Calculate the parameters for the plane d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; - //Calculate the position on the line that intersects the plane + //Calculate the position on the line that intersects the plane denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); - if (abs(denom) < 0.0000001) // Line and plane don't intersect + if (abs(denom) < 0.0000001) // Line and plane don't intersect return 0; mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; p->x = p1.x + mu * (p2.x - p1.x); p->y = p1.y + mu * (p2.y - p1.y); p->z = p1.z + mu * (p2.z - p1.z); - if (mu < 0 || mu > 1) // Intersection not along line segment + if (mu < 0 || mu > 1) // Intersection not along line segment return 0; - + if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;} return 1; } @@ -545,20 +542,20 @@ float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc, XYZ n, XYZ *p) float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc, XYZ *n, XYZ *p) { - //Calculate the parameters for the plane + //Calculate the parameters for the plane d = - n->x * pa->x - n->y * pa->y - n->z * pa->z; - //Calculate the position on the line that intersects the plane + //Calculate the position on the line that intersects the plane denom = n->x * (p2->x - p1->x) + n->y * (p2->y - p1->y) + n->z * (p2->z - p1->z); - if (abs(denom) < 0.0000001) // Line and plane don't intersect + if (abs(denom) < 0.0000001) // Line and plane don't intersect return 0; mu = - (d + n->x * p1->x + n->y * p1->y + n->z * p1->z) / denom; p->x = p1->x + mu * (p2->x - p1->x); p->y = p1->y + mu * (p2->y - p1->y); p->z = p1->z + mu * (p2->z - p1->z); - if (mu < 0 || mu > 1) // Intersection not along line segment + if (mu < 0 || mu > 1) // Intersection not along line segment return 0; - + if(!PointInTriangle( p, *n, pa, pb, pc)){return 0;} return 1; } @@ -568,7 +565,7 @@ void ReflectVector(XYZ *vel, XYZ *n) XYZ vn; XYZ vt; float dotprod; - + dotprod=dotproduct(*n,*vel); vn.x=n->x*dotprod; vn.y=n->y*dotprod; @@ -577,7 +574,7 @@ void ReflectVector(XYZ *vel, XYZ *n) vt.x=vel->x-vn.x; vt.y=vel->y-vn.y; vt.z=vel->z-vn.z; - + vel->x = vt.x - vn.x; vel->y = vt.y - vn.y; vel->z = vt.z - vn.z; @@ -597,7 +594,6 @@ float findLength(XYZ point1){ return sqrt((point1.x)*(point1.x)+(point1.y)*(point1.y)+(point1.z)*(point1.z)); } - float findLengthfast(XYZ point1){ return((point1.x)*(point1.x)+(point1.y)*(point1.y)+(point1.z)*(point1.z)); } @@ -620,29 +616,28 @@ XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){ zang*=6.283185; zang/=360; } - - + if(yang){ newpoint.z=thePoint.z*cos(yang)-thePoint.x*sin(yang); newpoint.x=thePoint.z*sin(yang)+thePoint.x*cos(yang); thePoint.z=newpoint.z; thePoint.x=newpoint.x; } - + if(zang){ newpoint.x=thePoint.x*cos(zang)-thePoint.y*sin(zang); newpoint.y=thePoint.y*cos(zang)+thePoint.x*sin(zang); thePoint.x=newpoint.x; thePoint.y=newpoint.y; } - + if(xang){ newpoint.y=thePoint.y*cos(xang)-thePoint.z*sin(xang); newpoint.z=thePoint.y*sin(xang)+thePoint.z*cos(xang); thePoint.z=newpoint.z; - thePoint.y=newpoint.y; + thePoint.y=newpoint.y; } - + return thePoint; } @@ -692,31 +687,30 @@ bool sphere_line_intersection ( XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang){ XYZ newpoint; XYZ oldpoint; - + oldpoint=thePoint; - + if(yang!=0){ newpoint.z=oldpoint.z*cos(yang)-oldpoint.x*sin(yang); newpoint.x=oldpoint.z*sin(yang)+oldpoint.x*cos(yang); oldpoint.z=newpoint.z; oldpoint.x=newpoint.x; } - + if(zang!=0){ newpoint.x=oldpoint.x*cos(zang)-oldpoint.y*sin(zang); newpoint.y=oldpoint.y*cos(zang)+oldpoint.x*sin(zang); oldpoint.x=newpoint.x; oldpoint.y=newpoint.y; } - + if(xang!=0){ newpoint.y=oldpoint.y*cos(xang)-oldpoint.z*sin(xang); newpoint.z=oldpoint.y*sin(xang)+oldpoint.z*cos(xang); oldpoint.z=newpoint.z; - oldpoint.y=newpoint.y; + oldpoint.y=newpoint.y; } - + return oldpoint; - -} +} |