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