aboutsummaryrefslogtreecommitdiff
path: root/src/Quaternions.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/Quaternions.cpp')
-rw-r--r--src/Quaternions.cpp388
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;
-
-}
generated by cgit v1.2.3 (git 2.25.1) at 2026-01-14 14:43:56 +0000