From 049a84956400a4f764a850ec7c6e42c43a506c84 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 5 Jun 2023 14:38:44 +0100 Subject: Moved hamilton tools to compat so they can be used more widely --- compat/hamilton-tools.cpp | 135 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 135 insertions(+) create mode 100644 compat/hamilton-tools.cpp (limited to 'compat/hamilton-tools.cpp') diff --git a/compat/hamilton-tools.cpp b/compat/hamilton-tools.cpp new file mode 100644 index 00000000..e18082a8 --- /dev/null +++ b/compat/hamilton-tools.cpp @@ -0,0 +1,135 @@ +#include "hamilton-tools.h" +#include + +double VectorLength(const tVector v) +{ + return(sqrt(v.v[0]*v.v[0] + v.v[1]*v.v[1] + v.v[2]*v.v[2])); +} + +double sqr(const double v) { return(v*v); } + +double VectorDistance(const double v1[], const tVector v2) +{ + return(sqrt(sqr(v2.v[0]-v1[0])+sqr(v2.v[1]-v1[1])+sqr(v2.v[2]-v1[2]))); +} + +tVector Lerp(const tVector s, const double d[], const double alpha) +{ + tVector V; + V.v[0] = s.v[0] + (d[0] - s.v[0]) * alpha; + V.v[1] = s.v[1] + (d[1] - s.v[1]) * alpha; + V.v[2] = s.v[2] + (d[2] - s.v[2]) * alpha; + return(V); +} + +tQuat QuatFromAngleAxe(const double angle, const tVector axe) +{ + double a = TO_RAD * 0.5 * angle; + double d = sin(a) / VectorLength(axe); + return ( tQuat ( + axe.v[0] * d, + axe.v[1] * d, + axe.v[2] * d, + cos(a) + ) + ); +} + +tQuat QuatMultiply(const tQuat qL, const tQuat qR) +{ + tQuat Q; + Q.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y; + Q.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z; + Q.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x; + Q.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z; + return(Q); +} + +double AngleBetween(const tQuat S, const tQuat D) +{ + return( TO_DEG * 2*acos(fabs(S.x*D.x + S.y*D.y + S.z*D.z + S.w*D.w)) ); +} + +tQuat QuatFromYPR(const double YPR[]) +{ + tQuat Q, Qp, Qy; + Q = QuatFromAngleAxe( -YPR[2], {0, 0, 1} ); //Roll, Z axe + Qp = QuatFromAngleAxe( -YPR[1], {1, 0, 0} ); //Pitch, X axe + Qy = QuatFromAngleAxe( -YPR[0], {0, 1, 0} ); //Yaw, Y axe + + Q = QuatMultiply(Qp, Q); + return(QuatMultiply(Qy, Q)); +} + +void Normalize(tQuat Q) +{ + double m = sqrt(Q.x*Q.x + Q.y*Q.y + Q.z*Q.z + Q.w*Q.w); + if (m > EPSILON) + { + m = 1 / m; + Q.x = Q.x * m; + Q.y = Q.y * m; + Q.z = Q.z * m; + Q.w = Q.w * m; + } + else Q = tQuat(0, 0, 0, 1); +} + +tQuat Slerp(const tQuat S, const tQuat D, const double alpha) +{ + // calc cosine of half angle + double cosin = S.x*D.x + S.y*D.y + S.z*D.z + S.w*D.w; + + // select nearest rotation direction + tQuat Q; + if (cosin < 0) + { + cosin = - cosin; + Q.x = - D.x; + Q.y = - D.y; + Q.z = - D.z; + Q.w = - D.w; + } + else Q = D; + + // calculate coefficients + double scale0, scale1; + if ((1.0 - cosin) > EPSILON) + { + double omega = acos(cosin); + double sinus = 1 / sin(omega); + scale0 = sin((1.0 - alpha) * omega) * sinus; + scale1 = sin(alpha * omega)* sinus; + } + else + { + scale0 = 1.0 - alpha; + scale1 = alpha; + } + + Q.x = scale0 * S.x + scale1 * Q.x; + Q.y = scale0 * S.y + scale1 * Q.y; + Q.z = scale0 * S.z + scale1 * Q.z; + Q.w = scale0 * S.w + scale1 * Q.w; + + Normalize(Q); + + return( Q ); +} + +void QuatToYPR(const tQuat Q, double YPR[]) +{ + const double xx = Q.x * Q.x; + const double xy = Q.x * Q.y; + const double xz = Q.x * Q.z; + const double xw = Q.x * Q.w; + const double yy = Q.y * Q.y; + const double yz = Q.y * Q.z; + const double yw = Q.y * Q.w; + const double zz = Q.z * Q.z; + const double zw = Q.z * Q.w; + + YPR[0] = TO_DEG * ( -atan2( 2 * ( xz + yw ), 1 - 2 * ( xx + yy ) )); + YPR[1] = TO_DEG * ( asin ( 2 * ( yz - xw ) )); + YPR[2] = TO_DEG * ( -atan2( 2 * ( xy + zw ), 1 - 2 * ( xx + zz ) )); +} -- cgit v1.2.3 From 8af36d5f6590b4f49acc288f9113cca4df3c99a0 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 5 Jun 2023 15:26:24 +0100 Subject: Changed hamiltons tools to use pass by reference --- compat/hamilton-tools.cpp | 18 +++++++++--------- compat/hamilton-tools.h | 10 +++++----- 2 files changed, 14 insertions(+), 14 deletions(-) (limited to 'compat/hamilton-tools.cpp') diff --git a/compat/hamilton-tools.cpp b/compat/hamilton-tools.cpp index e18082a8..dfbb4630 100644 --- a/compat/hamilton-tools.cpp +++ b/compat/hamilton-tools.cpp @@ -1,19 +1,19 @@ #include "hamilton-tools.h" #include -double VectorLength(const tVector v) +double VectorLength(const tVector& v) { return(sqrt(v.v[0]*v.v[0] + v.v[1]*v.v[1] + v.v[2]*v.v[2])); } double sqr(const double v) { return(v*v); } -double VectorDistance(const double v1[], const tVector v2) +double VectorDistance(const double v1[], const tVector& v2) { return(sqrt(sqr(v2.v[0]-v1[0])+sqr(v2.v[1]-v1[1])+sqr(v2.v[2]-v1[2]))); } -tVector Lerp(const tVector s, const double d[], const double alpha) +tVector Lerp(const tVector& s, const double d[], const double alpha) { tVector V; V.v[0] = s.v[0] + (d[0] - s.v[0]) * alpha; @@ -22,7 +22,7 @@ tVector Lerp(const tVector s, const double d[], const double alpha) return(V); } -tQuat QuatFromAngleAxe(const double angle, const tVector axe) +tQuat QuatFromAngleAxe(const double angle, const tVector& axe) { double a = TO_RAD * 0.5 * angle; double d = sin(a) / VectorLength(axe); @@ -35,7 +35,7 @@ tQuat QuatFromAngleAxe(const double angle, const tVector axe) ); } -tQuat QuatMultiply(const tQuat qL, const tQuat qR) +tQuat QuatMultiply(const tQuat& qL, const tQuat& qR) { tQuat Q; Q.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y; @@ -45,7 +45,7 @@ tQuat QuatMultiply(const tQuat qL, const tQuat qR) return(Q); } -double AngleBetween(const tQuat S, const tQuat D) +double AngleBetween(const tQuat& S, const tQuat& D) { return( TO_DEG * 2*acos(fabs(S.x*D.x + S.y*D.y + S.z*D.z + S.w*D.w)) ); } @@ -61,7 +61,7 @@ tQuat QuatFromYPR(const double YPR[]) return(QuatMultiply(Qy, Q)); } -void Normalize(tQuat Q) +void Normalize(tQuat& Q) { double m = sqrt(Q.x*Q.x + Q.y*Q.y + Q.z*Q.z + Q.w*Q.w); if (m > EPSILON) @@ -75,7 +75,7 @@ void Normalize(tQuat Q) else Q = tQuat(0, 0, 0, 1); } -tQuat Slerp(const tQuat S, const tQuat D, const double alpha) +tQuat Slerp(const tQuat& S, const tQuat& D, const double alpha) { // calc cosine of half angle double cosin = S.x*D.x + S.y*D.y + S.z*D.z + S.w*D.w; @@ -117,7 +117,7 @@ tQuat Slerp(const tQuat S, const tQuat D, const double alpha) return( Q ); } -void QuatToYPR(const tQuat Q, double YPR[]) +void QuatToYPR(const tQuat& Q, double YPR[]) { const double xx = Q.x * Q.x; const double xy = Q.x * Q.y; diff --git a/compat/hamilton-tools.h b/compat/hamilton-tools.h index 6bcd7b5d..73bc882c 100644 --- a/compat/hamilton-tools.h +++ b/compat/hamilton-tools.h @@ -21,9 +21,9 @@ struct tQuat {x = X; y = Y; z = Z; w = W;} }; -double OTR_COMPAT_EXPORT VectorDistance(const double v1[], const tVector v2); -tVector OTR_COMPAT_EXPORT Lerp (const tVector s, const double d[], const double alpha); +double OTR_COMPAT_EXPORT VectorDistance(const double v1[], const tVector& v2); +tVector OTR_COMPAT_EXPORT Lerp (const tVector& s, const double d[], const double alpha); tQuat OTR_COMPAT_EXPORT QuatFromYPR (const double YPR[]); -double OTR_COMPAT_EXPORT AngleBetween (const tQuat S, const tQuat D); -tQuat OTR_COMPAT_EXPORT Slerp (const tQuat S, const tQuat D, const double alpha); -void OTR_COMPAT_EXPORT QuatToYPR (const tQuat Q, double YPR[]); +double OTR_COMPAT_EXPORT AngleBetween (const tQuat& S, const tQuat& D); +tQuat OTR_COMPAT_EXPORT Slerp (const tQuat& S, const tQuat& D, const double alpha); +void OTR_COMPAT_EXPORT QuatToYPR (const tQuat& Q, double YPR[]); -- cgit v1.2.3 From 96f51ea8a769c2f49525e2424ace165ec3c05ef8 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 5 Jun 2023 15:52:26 +0100 Subject: Added new vector and quaternion functions and inlined many of them --- compat/hamilton-tools.cpp | 26 -------------------- compat/hamilton-tools.h | 60 +++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 53 insertions(+), 33 deletions(-) (limited to 'compat/hamilton-tools.cpp') diff --git a/compat/hamilton-tools.cpp b/compat/hamilton-tools.cpp index dfbb4630..eb0ef984 100644 --- a/compat/hamilton-tools.cpp +++ b/compat/hamilton-tools.cpp @@ -1,27 +1,6 @@ #include "hamilton-tools.h" #include -double VectorLength(const tVector& v) -{ - return(sqrt(v.v[0]*v.v[0] + v.v[1]*v.v[1] + v.v[2]*v.v[2])); -} - -double sqr(const double v) { return(v*v); } - -double VectorDistance(const double v1[], const tVector& v2) -{ - return(sqrt(sqr(v2.v[0]-v1[0])+sqr(v2.v[1]-v1[1])+sqr(v2.v[2]-v1[2]))); -} - -tVector Lerp(const tVector& s, const double d[], const double alpha) -{ - tVector V; - V.v[0] = s.v[0] + (d[0] - s.v[0]) * alpha; - V.v[1] = s.v[1] + (d[1] - s.v[1]) * alpha; - V.v[2] = s.v[2] + (d[2] - s.v[2]) * alpha; - return(V); -} - tQuat QuatFromAngleAxe(const double angle, const tVector& axe) { double a = TO_RAD * 0.5 * angle; @@ -45,11 +24,6 @@ tQuat QuatMultiply(const tQuat& qL, const tQuat& qR) return(Q); } -double AngleBetween(const tQuat& S, const tQuat& D) -{ - return( TO_DEG * 2*acos(fabs(S.x*D.x + S.y*D.y + S.z*D.z + S.w*D.w)) ); -} - tQuat QuatFromYPR(const double YPR[]) { tQuat Q, Qp, Qy; diff --git a/compat/hamilton-tools.h b/compat/hamilton-tools.h index 73bc882c..d4604301 100644 --- a/compat/hamilton-tools.h +++ b/compat/hamilton-tools.h @@ -11,7 +11,50 @@ struct tVector { double v[3]; tVector(double X = 0, double Y = 0, double Z = 0) {v[0]=X; v[1]=Y; v[2]=Z;} - tVector(double V[]) {v[0]=V[0]; v[1]=V[1]; v[2]=V[2];} + tVector(const double V[]) {v[0]=V[0]; v[1]=V[1]; v[2]=V[2];} + + void operator=(const tVector& other) + { + std::copy(other.v, other.v + 3, v); + } + inline const tVector operator+(const tVector& other) const + { + return tVector(v[0] + other.v[0], v[1] + other.v[1], v[2] + other.v[2]); + } + void operator+=(const tVector& other) + { + v[0] += other.v[0]; + v[1] += other.v[1]; + v[2] += other.v[2]; + } + const tVector operator-(const tVector& other) const + { + return tVector(v[0] - other.v[0], v[1] - other.v[1], v[2] - other.v[2]); + } + void operator-=(const tVector& other) + { + v[0] -= other.v[0]; + v[1] -= other.v[1]; + v[2] -= other.v[2]; + } + const tVector operator*(double scalar) const + { + return tVector(v[0] * scalar, v[1] * scalar, v[2] * scalar); + } + void operator*=(double scalar) + { + v[0] *= scalar; + v[1] *= scalar; + v[2] *= scalar; + } + const tVector operator/(double scalar) const + { + return *this * (1.0 / scalar); + } + void operator/= (double scalar) + { + *this *= 1.0 / scalar; + } }; struct tQuat @@ -21,9 +64,12 @@ struct tQuat {x = X; y = Y; z = Z; w = W;} }; -double OTR_COMPAT_EXPORT VectorDistance(const double v1[], const tVector& v2); -tVector OTR_COMPAT_EXPORT Lerp (const tVector& s, const double d[], const double alpha); -tQuat OTR_COMPAT_EXPORT QuatFromYPR (const double YPR[]); -double OTR_COMPAT_EXPORT AngleBetween (const tQuat& S, const tQuat& D); -tQuat OTR_COMPAT_EXPORT Slerp (const tQuat& S, const tQuat& D, const double alpha); -void OTR_COMPAT_EXPORT QuatToYPR (const tQuat& Q, double YPR[]); +inline double VectorLength(const tVector& v) { return sqrt(v.v[0] * v.v[0] + v.v[1] * v.v[1] + v.v[2] * v.v[2]); } +inline double VectorDistance(const tVector& v1, const tVector& v2) { return VectorLength(v2 - v1); } +inline tVector Lerp(const tVector& s, const tVector& d, const double alpha) { return s + (d - s) * alpha; } +tQuat OTR_COMPAT_EXPORT QuatFromYPR(const double YPR[]); +tQuat OTR_COMPAT_EXPORT QuatMultiply(const tQuat& qL, const tQuat& qR); +inline tQuat QuatDivide(const tQuat& qL, const tQuat& qR) { return QuatMultiply(qL, tQuat(-qR.x, -qR.y, -qR.z, qR.w)); } +inline double AngleBetween(const tQuat& S, const tQuat& D) { return TO_DEG * 2 * acos(fabs(S.x * D.x + S.y * D.y + S.z * D.z + S.w * D.w)); } +tQuat OTR_COMPAT_EXPORT Slerp(const tQuat& S, const tQuat& D, const double alpha); +void OTR_COMPAT_EXPORT QuatToYPR(const tQuat& Q, double YPR[]); -- cgit v1.2.3