diff options
Diffstat (limited to 'src/Quaternions.cpp')
| -rw-r--r-- | src/Quaternions.cpp | 388 |
1 files changed, 5 insertions, 383 deletions
diff --git a/src/Quaternions.cpp b/src/Quaternions.cpp index 90c7502..fc38c44 100644 --- a/src/Quaternions.cpp +++ b/src/Quaternions.cpp @@ -1,24 +1,6 @@ -#include "Quaternions.h" +#include <cmath> -// Functions -quaternion Quat_Mult(quaternion q1, quaternion q2) -{ - quaternion QResult; - float a, b, c, d, e, f, g, h; - a = (q1.w + q1.x) * (q2.w + q2.x); - b = (q1.z - q1.y) * (q2.y - q2.z); - c = (q1.w - q1.x) * (q2.y + q2.z); - d = (q1.y + q1.z) * (q2.w - q2.x); - e = (q1.x + q1.z) * (q2.x + q2.y); - f = (q1.x - q1.z) * (q2.x - q2.y); - g = (q1.w + q1.y) * (q2.w - q2.z); - h = (q1.w - q1.y) * (q2.w + q2.z); - QResult.w = b + (-e - f + g + h) / 2; - QResult.x = a - (e + f + g + h) / 2; - QResult.y = c + (e - f + g - h) / 2; - QResult.z = d + (e - f - g + h) / 2; - return QResult; -} +#include "Quaternions.h" XYZ XYZ::operator+(XYZ add){ XYZ ne; @@ -98,177 +80,11 @@ void XYZ::operator=(float add){ z=add; } -void XYZ::vec(Vector add){ - x=add.x; - y=add.y; - z=add.z; -} - bool XYZ::operator==(XYZ add){ if(x==add.x&&y==add.y&&z==add.z)return 1; return 0; } -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) - { - fourD = 2.0 * sqrt(Tr); - q[3] = fourD/4.0; - q[0] = (m[2][1] - m[1][2]) / fourD; - q[1] = (m[0][2] - m[2][0]) / fourD; - q[2] = (m[1][0] - m[0][1]) / fourD; - } - else - { - if (m[0][0] > m[1][1]) - { - i = 0; - } - else - { - i = 1; - } - if (m[2][2] > m[i][i]) - { - i = 2; - } - j = (i+1)%3; - k = (j+1)%3; - fourD = 2.0 * sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0); - q[i] = fourD / 4.0; - q[j] = (m[j][i] + m[i][j]) / fourD; - 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]; - Quat.w = q[3]; - return Quat; -} -void Quat_2_Matrix(quaternion Quat, Matrix_t m) -{ - // From the GLVelocity site (http://glvelocity.gamedev.net) - float fW = Quat.w; - float fX = Quat.x; - float fY = Quat.y; - float fZ = Quat.z; - float fXX = fX * fX; - float fYY = fY * fY; - float fZZ = fZ * fZ; - m[0][0] = 1.0f - 2.0f * (fYY + fZZ); - m[1][0] = 2.0f * (fX * fY + fW * fZ); - m[2][0] = 2.0f * (fX * fZ - fW * fY); - m[3][0] = 0.0f; - m[0][1] = 2.0f * (fX * fY - fW * fZ); - m[1][1] = 1.0f - 2.0f * (fXX + fZZ); - m[2][1] = 2.0f * (fY * fZ + fW * fX); - m[3][1] = 0.0f; - m[0][2] = 2.0f * (fX * fZ + fW * fY); - m[1][2] = 2.0f * (fX * fZ - fW * fX); - m[2][2] = 1.0f - 2.0f * (fXX + fYY); - m[3][2] = 0.0f; - m[0][3] = 0.0f; - m[1][3] = 0.0f; - m[2][3] = 0.0f; - m[3][3] = 1.0f; -} -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); - Quat.w = cos(Ang_Ax.angle / 2); - return Quat; -} -angle_axis Quat_2_AA(quaternion Quat) -{ - angle_axis Ang_Ax; - float scale, tw; - tw = (float)acos(Quat.w) * 2; - scale = (float)sin(tw / 2.0); - 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; -} - -quaternion To_Quat(int In_Degrees, euler Euler) -{ - // From the gamasutra quaternion article - quaternion Quat; - float cr, cp, cy, sr, sp, sy, cpcy, spsy; - //If we are in Degree mode, convert to Radians - if (In_Degrees) { - Euler.x = Euler.x * (float)PI / 180; - Euler.y = Euler.y * (float)PI / 180; - Euler.z = Euler.z * (float)PI / 180; - } - //Calculate trig identities - //Formerly roll, pitch, yaw - cr = float(cos(Euler.x/2)); - cp = float(cos(Euler.y/2)); - cy = float(cos(Euler.z/2)); - 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 + - Quat.w * Quat.w; - Quat.x = float(Quat.x / norm); - Quat.y = float(Quat.y / norm); - Quat.z = float(Quat.z / norm); - Quat.w = float(Quat.w / norm); - return Quat; -} - -XYZ Quat2Vector(quaternion Quat) -{ - QNormalize(Quat); - - float fW = Quat.w; - 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; -} - void CrossProduct(XYZ P, XYZ Q, XYZ *V){ V->x = P.y * Q.z - P.z * Q.y; V->y = P.z * Q.x - P.x * Q.z; @@ -283,14 +99,6 @@ void Normalise(XYZ *vectory) { vectory->z /= d; } -float normaldotproduct(XYZ point1, XYZ point2){ - GLfloat returnvalue; - Normalise(&point1); - Normalise(&point2); - returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z); - return returnvalue; -} - extern float u0, u1, u2; extern float v0, v1, v2; extern float a, b; @@ -302,99 +110,6 @@ extern float p2v[3]; extern float p3v[3]; 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)) - 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]; - v1 = p2v[j] - p1v[j]; - u2 = p3v[i] - p1v[i]; - v2 = p3v[j] - p1v[j]; - - if (u1 > -1.0e-05f && u1 < 1.0e-05f)// == 0.0f) - { - b = u0 / u2; - if (0.0f <= b && b <= 1.0f) - { - a = (v0 - b * v2) / v1; - if ((a >= 0.0f) && (( a + b ) <= 1.0f)) - bInter = 1; - } - } - else - { - b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1); - if (0.0f <= b && b <= 1.0f) - { - a = (u0 - b * u2) / u1; - if ((a >= 0.0f) && (( a + b ) <= 1.0f )) - bInter = 1; - } - } - - return bInter; -} - -bool LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector *p) -{ - float d; - float denom, mu; - Vector n, pa1, pa2, pa3; - - //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 - 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 - 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 - 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; @@ -459,65 +174,11 @@ bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3) return bInter; } -bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p) -{ - float d; - float denom, mu; - XYZ n; - - //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 - 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 - 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 - return 0; - - if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;} - - return 1; -} - extern float d; extern float a1,a2,a3; extern float total,denom,mu; 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 - 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 - 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 - 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 - 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) { @@ -581,19 +242,13 @@ void ReflectVector(XYZ *vel, XYZ *n) } float dotproduct(XYZ point1, XYZ point2){ - GLfloat returnvalue; - returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z); - return returnvalue; + return point1.x * point2.x + point1.y * point2.y + point1.z * point2.z; } float findDistance(XYZ point1, XYZ point2){ return sqrt((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z)); } -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)); } @@ -643,10 +298,8 @@ XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){ float square( float f ) { return (f*f) ;} -bool sphere_line_intersection ( - float x1, float y1 , float z1, - float x2, float y2 , float z2, - float x3, float y3 , float z3, float r ) +bool sphere_line_intersection(float x1, float y1, float z1, + float x2, float y2, float z2, float x3, float y3, float z3, float r) { // x1,y1,z1 P1 coordinates (point of line) @@ -683,34 +336,3 @@ bool sphere_line_intersection ( return(1); } - -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; - } - - return oldpoint; - -} |