diff --git a/src/FoundationClasses/TKMath/gp/FILES.cmake b/src/FoundationClasses/TKMath/gp/FILES.cmake index c56542a066..e3694f3ac3 100644 --- a/src/FoundationClasses/TKMath/gp/FILES.cmake +++ b/src/FoundationClasses/TKMath/gp/FILES.cmake @@ -78,7 +78,6 @@ set(OCCT_gp_FILES gp_Vec2f.hxx gp_Vec3f.hxx gp_VectorWithNullMagnitude.hxx - gp_XY.cxx gp_XY.hxx gp_XYZ.cxx gp_XYZ.hxx diff --git a/src/FoundationClasses/TKMath/gp/gp_Mat.cxx b/src/FoundationClasses/TKMath/gp/gp_Mat.cxx index 56021c24d5..e32ce66b67 100644 --- a/src/FoundationClasses/TKMath/gp/gp_Mat.cxx +++ b/src/FoundationClasses/TKMath/gp/gp_Mat.cxx @@ -88,12 +88,13 @@ void gp_Mat::SetCross(const gp_XYZ& theRef) const Standard_Real Y = theRef.Y(); const Standard_Real Z = theRef.Z(); myMat[0][0] = myMat[1][1] = myMat[2][2] = 0.0; - myMat[0][1] = -Z; - myMat[0][2] = Y; - myMat[1][2] = -X; - myMat[1][0] = Z; - myMat[2][0] = -Y; - myMat[2][1] = X; + + myMat[0][1] = -Z; + myMat[0][2] = Y; + myMat[1][2] = -X; + myMat[1][0] = Z; + myMat[2][0] = -Y; + myMat[2][1] = X; } //================================================================================================= @@ -103,37 +104,58 @@ void gp_Mat::SetDot(const gp_XYZ& theRef) const Standard_Real X = theRef.X(); const Standard_Real Y = theRef.Y(); const Standard_Real Z = theRef.Z(); - myMat[0][0] = X * X; - myMat[1][1] = Y * Y; - myMat[2][2] = Z * Z; - myMat[0][1] = X * Y; - myMat[0][2] = X * Z; - myMat[1][2] = Y * Z; - myMat[1][0] = myMat[0][1]; - myMat[2][0] = myMat[0][2]; - myMat[2][1] = myMat[1][2]; + + myMat[0][0] = X * X; + myMat[1][1] = Y * Y; + myMat[2][2] = Z * Z; + myMat[0][1] = X * Y; + myMat[0][2] = X * Z; + myMat[1][2] = Y * Z; + + myMat[1][0] = myMat[0][1]; + myMat[2][0] = myMat[0][2]; + myMat[2][1] = myMat[1][2]; } //================================================================================================= void gp_Mat::SetRotation(const gp_XYZ& theAxis, const Standard_Real theAng) { - // Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M - // avec M . XYZ = Axis ^ XYZ + // Rodrigues' rotation formula: R = I + sin(θ)K + (1-cos(θ))K² + // Where K is the skew-symmetric matrix of the normalized axis const gp_XYZ aV = theAxis.Normalized(); - SetCross(aV); - Multiply(sin(theAng)); - gp_Mat aTemp; - aTemp.SetScale(1.0); - Add(aTemp); + const Standard_Real A = aV.X(); const Standard_Real B = aV.Y(); const Standard_Real C = aV.Z(); - aTemp.SetRow(1, gp_XYZ(-C * C - B * B, A * B, A * C)); - aTemp.SetRow(2, gp_XYZ(A * B, -A * A - C * C, B * C)); - aTemp.SetRow(3, gp_XYZ(A * C, B * C, -A * A - B * B)); - aTemp.Multiply(1.0 - cos(theAng)); - Add(aTemp); + + // Precompute trigonometric values + const Standard_Real aCos = cos(theAng); + const Standard_Real aSin = sin(theAng); + const Standard_Real aOmCos = 1.0 - aCos; // One minus cosine + + // Precompute terms + const Standard_Real A2 = A * A; + const Standard_Real B2 = B * B; + const Standard_Real C2 = C * C; + const Standard_Real AB = A * B; + const Standard_Real AC = A * C; + const Standard_Real BC = B * C; + + // Direct matrix computation: R = I + sin(θ)K + (1-cos(θ))K² + // K² diagonal terms are -(sum of other two squared components) + // K² off-diagonal terms are products of components + myMat[0][0] = 1.0 + aOmCos * (-(B2 + C2)); + myMat[0][1] = aOmCos * AB - aSin * C; + myMat[0][2] = aOmCos * AC + aSin * B; + + myMat[1][0] = aOmCos * AB + aSin * C; + myMat[1][1] = 1.0 + aOmCos * (-(A2 + C2)); + myMat[1][2] = aOmCos * BC - aSin * A; + + myMat[2][0] = aOmCos * AC - aSin * B; + myMat[2][1] = aOmCos * BC + aSin * A; + myMat[2][2] = 1.0 + aOmCos * (-(A2 + B2)); } //================================================================================================= @@ -211,55 +233,49 @@ gp_XYZ gp_Mat::Row(const Standard_Integer theRow) const void gp_Mat::Invert() { - Standard_Real aNewMat[3][3]; - // calcul de la transposee de la commatrice - aNewMat[0][0] = myMat[1][1] * myMat[2][2] - myMat[1][2] * myMat[2][1]; - aNewMat[1][0] = -(myMat[1][0] * myMat[2][2] - myMat[2][0] * myMat[1][2]); - aNewMat[2][0] = myMat[1][0] * myMat[2][1] - myMat[2][0] * myMat[1][1]; - aNewMat[0][1] = -(myMat[0][1] * myMat[2][2] - myMat[2][1] * myMat[0][2]); - aNewMat[1][1] = myMat[0][0] * myMat[2][2] - myMat[2][0] * myMat[0][2]; - aNewMat[2][1] = -(myMat[0][0] * myMat[2][1] - myMat[2][0] * myMat[0][1]); - aNewMat[0][2] = myMat[0][1] * myMat[1][2] - myMat[1][1] * myMat[0][2]; - aNewMat[1][2] = -(myMat[0][0] * myMat[1][2] - myMat[1][0] * myMat[0][2]); - aNewMat[2][2] = myMat[0][0] * myMat[1][1] - myMat[0][1] * myMat[1][0]; - Standard_Real aDet = - myMat[0][0] * aNewMat[0][0] + myMat[0][1] * aNewMat[1][0] + myMat[0][2] * aNewMat[2][0]; + // Optimized matrix inversion using cached elements + const Standard_Real a00 = myMat[0][0], a01 = myMat[0][1], a02 = myMat[0][2]; + const Standard_Real a10 = myMat[1][0], a11 = myMat[1][1], a12 = myMat[1][2]; + const Standard_Real a20 = myMat[2][0], a21 = myMat[2][1], a22 = myMat[2][2]; + + // Compute adjugate matrix (transpose of cofactor matrix) + const Standard_Real adj00 = a11 * a22 - a12 * a21; + const Standard_Real adj10 = a12 * a20 - a10 * a22; + const Standard_Real adj20 = a10 * a21 - a11 * a20; + const Standard_Real adj01 = a02 * a21 - a01 * a22; + const Standard_Real adj11 = a00 * a22 - a02 * a20; + const Standard_Real adj21 = a01 * a20 - a00 * a21; + const Standard_Real adj02 = a01 * a12 - a02 * a11; + const Standard_Real adj12 = a02 * a10 - a00 * a12; + const Standard_Real adj22 = a00 * a11 - a01 * a10; + + // Compute determinant using first row expansion (reuse computed cofactors) + const Standard_Real aDet = a00 * adj00 + a01 * adj10 + a02 * adj20; + Standard_ConstructionError_Raise_if(Abs(aDet) <= gp::Resolution(), "gp_Mat::Invert() - matrix has zero determinant"); - aDet = 1.0e0 / aDet; - myMat[0][0] = aNewMat[0][0]; - myMat[1][0] = aNewMat[1][0]; - myMat[2][0] = aNewMat[2][0]; - myMat[0][1] = aNewMat[0][1]; - myMat[1][1] = aNewMat[1][1]; - myMat[2][1] = aNewMat[2][1]; - myMat[0][2] = aNewMat[0][2]; - myMat[1][2] = aNewMat[1][2]; - myMat[2][2] = aNewMat[2][2]; - Multiply(aDet); + + // Compute inverse: inv(A) = adj(A) / det(A) + const Standard_Real aInvDet = 1.0 / aDet; + + // Direct assignment with scaling + myMat[0][0] = adj00 * aInvDet; + myMat[1][0] = adj10 * aInvDet; + myMat[2][0] = adj20 * aInvDet; + myMat[0][1] = adj01 * aInvDet; + myMat[1][1] = adj11 * aInvDet; + myMat[2][1] = adj21 * aInvDet; + myMat[0][2] = adj02 * aInvDet; + myMat[1][2] = adj12 * aInvDet; + myMat[2][2] = adj22 * aInvDet; } //================================================================================================= gp_Mat gp_Mat::Inverted() const { - gp_Mat aNewMat; - // calcul de la transposee de la commatrice - aNewMat.myMat[0][0] = myMat[1][1] * myMat[2][2] - myMat[1][2] * myMat[2][1]; - aNewMat.myMat[1][0] = -(myMat[1][0] * myMat[2][2] - myMat[2][0] * myMat[1][2]); - aNewMat.myMat[2][0] = myMat[1][0] * myMat[2][1] - myMat[2][0] * myMat[1][1]; - aNewMat.myMat[0][1] = -(myMat[0][1] * myMat[2][2] - myMat[2][1] * myMat[0][2]); - aNewMat.myMat[1][1] = myMat[0][0] * myMat[2][2] - myMat[2][0] * myMat[0][2]; - aNewMat.myMat[2][1] = -(myMat[0][0] * myMat[2][1] - myMat[2][0] * myMat[0][1]); - aNewMat.myMat[0][2] = myMat[0][1] * myMat[1][2] - myMat[1][1] * myMat[0][2]; - aNewMat.myMat[1][2] = -(myMat[0][0] * myMat[1][2] - myMat[1][0] * myMat[0][2]); - aNewMat.myMat[2][2] = myMat[0][0] * myMat[1][1] - myMat[0][1] * myMat[1][0]; - Standard_Real aDet = myMat[0][0] * aNewMat.myMat[0][0] + myMat[0][1] * aNewMat.myMat[1][0] - + myMat[0][2] * aNewMat.myMat[2][0]; - Standard_ConstructionError_Raise_if(Abs(aDet) <= gp::Resolution(), - "gp_Mat::Inverted() - matrix has zero determinant"); - aDet = 1.0e0 / aDet; - aNewMat.Multiply(aDet); + gp_Mat aNewMat(*this); + aNewMat.Invert(); return aNewMat; } @@ -267,37 +283,45 @@ gp_Mat gp_Mat::Inverted() const void gp_Mat::Power(const Standard_Integer theN) { - if (theN == 1) - { - } - else if (theN == 0) + // Optimized matrix exponentiation using fast binary exponentiation + if (theN == 0) { SetIdentity(); + return; } - else if (theN == -1) + + if (theN == 1) + { + return; // Matrix is already this^1 + } + + if (theN == -1) + { + Invert(); + return; + } + + // Handle negative powers: A^(-n) = (A^(-1))^n + const Standard_Boolean isNegative = (theN < 0); + if (isNegative) { Invert(); } - else + + // Fast binary exponentiation for |theN| > 1 + Standard_Integer power = isNegative ? -theN : theN; + gp_Mat aBase = *this; // Base for exponentiation + SetIdentity(); // Result starts as identity + + // Binary exponentiation: repeatedly square base and multiply when bit is set + while (power > 0) { - if (theN < 0) + if (power & 1) // If current bit is 1 { - Invert(); - } - Standard_Integer Npower = theN; - if (Npower < 0) - Npower = -Npower; - Npower--; - gp_Mat aTemp = *this; - for (;;) - { - if (IsOdd(Npower)) - Multiply(aTemp); - if (Npower == 1) - break; - aTemp.Multiply(aTemp); - Npower >>= 1; + Multiply(aBase); } + aBase.Multiply(aBase); // Square the base + power >>= 1; // Move to next bit } } diff --git a/src/FoundationClasses/TKMath/gp/gp_Mat.hxx b/src/FoundationClasses/TKMath/gp/gp_Mat.hxx index 6d22604701..0008aec9fb 100644 --- a/src/FoundationClasses/TKMath/gp/gp_Mat.hxx +++ b/src/FoundationClasses/TKMath/gp/gp_Mat.hxx @@ -141,9 +141,14 @@ public: //! Computes the determinant of the matrix. Standard_Real Determinant() const { - return myMat[0][0] * (myMat[1][1] * myMat[2][2] - myMat[2][1] * myMat[1][2]) - - myMat[0][1] * (myMat[1][0] * myMat[2][2] - myMat[2][0] * myMat[1][2]) - + myMat[0][2] * (myMat[1][0] * myMat[2][1] - myMat[2][0] * myMat[1][1]); + const Standard_Real a00 = myMat[0][0], a01 = myMat[0][1], a02 = myMat[0][2]; + const Standard_Real a10 = myMat[1][0], a11 = myMat[1][1], a12 = myMat[1][2]; + const Standard_Real a20 = myMat[2][0], a21 = myMat[2][1], a22 = myMat[2][2]; + // clang-format off + return a00 * (a11 * a22 - a21 * a12) + - a01 * (a10 * a22 - a20 * a12) + + a02 * (a10 * a21 - a20 * a11); + // clang-format on } //! Returns the main diagonal of the matrix. @@ -353,16 +358,8 @@ inline void gp_Mat::Add(const gp_Mat& theOther) //======================================================================= inline gp_Mat gp_Mat::Added(const gp_Mat& theOther) const { - gp_Mat aNewMat; - aNewMat.myMat[0][0] = myMat[0][0] + theOther.myMat[0][0]; - aNewMat.myMat[0][1] = myMat[0][1] + theOther.myMat[0][1]; - aNewMat.myMat[0][2] = myMat[0][2] + theOther.myMat[0][2]; - aNewMat.myMat[1][0] = myMat[1][0] + theOther.myMat[1][0]; - aNewMat.myMat[1][1] = myMat[1][1] + theOther.myMat[1][1]; - aNewMat.myMat[1][2] = myMat[1][2] + theOther.myMat[1][2]; - aNewMat.myMat[2][0] = myMat[2][0] + theOther.myMat[2][0]; - aNewMat.myMat[2][1] = myMat[2][1] + theOther.myMat[2][1]; - aNewMat.myMat[2][2] = myMat[2][2] + theOther.myMat[2][2]; + gp_Mat aNewMat(*this); + aNewMat.Add(theOther); return aNewMat; } @@ -396,23 +393,8 @@ inline void gp_Mat::Divide(const Standard_Real theScalar) //======================================================================= inline gp_Mat gp_Mat::Divided(const Standard_Real theScalar) const { - Standard_Real aVal = theScalar; - if (aVal < 0) - { - aVal = -aVal; - } - Standard_ConstructionError_Raise_if(aVal <= gp::Resolution(), "gp_Mat : Divide by 0"); - gp_Mat aNewMat; - const Standard_Real anUnSurScalar = 1.0 / theScalar; - aNewMat.myMat[0][0] = myMat[0][0] * anUnSurScalar; - aNewMat.myMat[0][1] = myMat[0][1] * anUnSurScalar; - aNewMat.myMat[0][2] = myMat[0][2] * anUnSurScalar; - aNewMat.myMat[1][0] = myMat[1][0] * anUnSurScalar; - aNewMat.myMat[1][1] = myMat[1][1] * anUnSurScalar; - aNewMat.myMat[1][2] = myMat[1][2] * anUnSurScalar; - aNewMat.myMat[2][0] = myMat[2][0] * anUnSurScalar; - aNewMat.myMat[2][1] = myMat[2][1] * anUnSurScalar; - aNewMat.myMat[2][2] = myMat[2][2] * anUnSurScalar; + gp_Mat aNewMat(*this); + aNewMat.Divide(theScalar); return aNewMat; } @@ -492,16 +474,8 @@ inline void gp_Mat::PreMultiply(const gp_Mat& theOther) //======================================================================= inline gp_Mat gp_Mat::Multiplied(const Standard_Real theScalar) const { - gp_Mat aNewMat; - aNewMat.myMat[0][0] = theScalar * myMat[0][0]; - aNewMat.myMat[0][1] = theScalar * myMat[0][1]; - aNewMat.myMat[0][2] = theScalar * myMat[0][2]; - aNewMat.myMat[1][0] = theScalar * myMat[1][0]; - aNewMat.myMat[1][1] = theScalar * myMat[1][1]; - aNewMat.myMat[1][2] = theScalar * myMat[1][2]; - aNewMat.myMat[2][0] = theScalar * myMat[2][0]; - aNewMat.myMat[2][1] = theScalar * myMat[2][1]; - aNewMat.myMat[2][2] = theScalar * myMat[2][2]; + gp_Mat aNewMat(*this); + aNewMat.Multiply(theScalar); return aNewMat; } @@ -545,16 +519,8 @@ inline void gp_Mat::Subtract(const gp_Mat& theOther) //======================================================================= inline gp_Mat gp_Mat::Subtracted(const gp_Mat& theOther) const { - gp_Mat aNewMat; - aNewMat.myMat[0][0] = myMat[0][0] - theOther.myMat[0][0]; - aNewMat.myMat[0][1] = myMat[0][1] - theOther.myMat[0][1]; - aNewMat.myMat[0][2] = myMat[0][2] - theOther.myMat[0][2]; - aNewMat.myMat[1][0] = myMat[1][0] - theOther.myMat[1][0]; - aNewMat.myMat[1][1] = myMat[1][1] - theOther.myMat[1][1]; - aNewMat.myMat[1][2] = myMat[1][2] - theOther.myMat[1][2]; - aNewMat.myMat[2][0] = myMat[2][0] - theOther.myMat[2][0]; - aNewMat.myMat[2][1] = myMat[2][1] - theOther.myMat[2][1]; - aNewMat.myMat[2][2] = myMat[2][2] - theOther.myMat[2][2]; + gp_Mat aNewMat(*this); + aNewMat.Subtract(theOther); return aNewMat; } @@ -573,16 +539,9 @@ __attribute__((optnone)) inline void gp_Mat::Transpose() { - Standard_Real aTemp; - aTemp = myMat[0][1]; - myMat[0][1] = myMat[1][0]; - myMat[1][0] = aTemp; - aTemp = myMat[0][2]; - myMat[0][2] = myMat[2][0]; - myMat[2][0] = aTemp; - aTemp = myMat[1][2]; - myMat[1][2] = myMat[2][1]; - myMat[2][1] = aTemp; + std::swap(myMat[0][1], myMat[1][0]); + std::swap(myMat[0][2], myMat[2][0]); + std::swap(myMat[1][2], myMat[2][1]); } //======================================================================= diff --git a/src/FoundationClasses/TKMath/gp/gp_Pnt.hxx b/src/FoundationClasses/TKMath/gp/gp_Pnt.hxx index 7d5124e802..706fb34a70 100644 --- a/src/FoundationClasses/TKMath/gp/gp_Pnt.hxx +++ b/src/FoundationClasses/TKMath/gp/gp_Pnt.hxx @@ -259,21 +259,7 @@ struct equal_to //======================================================================= inline Standard_Real gp_Pnt::Distance(const gp_Pnt& theOther) const { - Standard_Real aD = 0, aDD; - const gp_XYZ& aXYZ = theOther.coord; - aDD = coord.X(); - aDD -= aXYZ.X(); - aDD *= aDD; - aD += aDD; - aDD = coord.Y(); - aDD -= aXYZ.Y(); - aDD *= aDD; - aD += aDD; - aDD = coord.Z(); - aDD -= aXYZ.Z(); - aDD *= aDD; - aD += aDD; - return sqrt(aD); + return sqrt(SquareDistance(theOther)); } //======================================================================= @@ -282,21 +268,11 @@ inline Standard_Real gp_Pnt::Distance(const gp_Pnt& theOther) const //======================================================================= inline Standard_Real gp_Pnt::SquareDistance(const gp_Pnt& theOther) const { - Standard_Real aD = 0, aDD; - const gp_XYZ& XYZ = theOther.coord; - aDD = coord.X(); - aDD -= XYZ.X(); - aDD *= aDD; - aD += aDD; - aDD = coord.Y(); - aDD -= XYZ.Y(); - aDD *= aDD; - aD += aDD; - aDD = coord.Z(); - aDD -= XYZ.Z(); - aDD *= aDD; - aD += aDD; - return aD; + const gp_XYZ& aXYZ = theOther.coord; + const Standard_Real aDx = coord.X() - aXYZ.X(); + const Standard_Real aDy = coord.Y() - aXYZ.Y(); + const Standard_Real aDz = coord.Z() - aXYZ.Z(); + return aDx * aDx + aDy * aDy + aDz * aDz; } //======================================================================= diff --git a/src/FoundationClasses/TKMath/gp/gp_Vec.cxx b/src/FoundationClasses/TKMath/gp/gp_Vec.cxx index 1af6d16735..1e31526aa6 100644 --- a/src/FoundationClasses/TKMath/gp/gp_Vec.cxx +++ b/src/FoundationClasses/TKMath/gp/gp_Vec.cxx @@ -29,117 +29,132 @@ #include #include -Standard_Boolean gp_Vec::IsEqual(const gp_Vec& Other, - const Standard_Real LinearTolerance, - const Standard_Real AngularTolerance) const +Standard_Boolean gp_Vec::IsEqual(const gp_Vec& theOther, + const Standard_Real theLinearTolerance, + const Standard_Real theAngularTolerance) const { - if (Magnitude() <= LinearTolerance || Other.Magnitude() <= LinearTolerance) + const Standard_Real aMagnitude = Magnitude(); + const Standard_Real anOtherMagnitude = theOther.Magnitude(); + + if (aMagnitude <= theLinearTolerance || anOtherMagnitude <= theLinearTolerance) { - Standard_Real val = Magnitude() - Other.Magnitude(); - if (val < 0) - val = -val; - return val <= LinearTolerance; + const Standard_Real aVal = Abs(aMagnitude - anOtherMagnitude); + return aVal <= theLinearTolerance; } else { - Standard_Real val = Magnitude() - Other.Magnitude(); - if (val < 0) - val = -val; - return val <= LinearTolerance && Angle(Other) <= AngularTolerance; + const Standard_Real aVal = Abs(aMagnitude - anOtherMagnitude); + return aVal <= theLinearTolerance && Angle(theOther) <= theAngularTolerance; } } -void gp_Vec::Mirror(const gp_Vec& V) +void gp_Vec::Mirror(const gp_Vec& theVec) { - Standard_Real D = V.coord.Modulus(); - if (D > gp::Resolution()) + const Standard_Real aMagnitude = theVec.coord.Modulus(); + if (aMagnitude > gp::Resolution()) { - const gp_XYZ& XYZ = V.coord; - Standard_Real A = XYZ.X() / D; - Standard_Real B = XYZ.Y() / D; - Standard_Real C = XYZ.Z() / D; - Standard_Real M1 = 2.0 * A * B; - Standard_Real M2 = 2.0 * A * C; - Standard_Real M3 = 2.0 * B * C; - Standard_Real X = coord.X(); - Standard_Real Y = coord.Y(); - Standard_Real Z = coord.Z(); - coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z); - coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z); - coord.SetZ(M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z); + const gp_XYZ& aMirrorVecXYZ = theVec.coord; + const Standard_Real aOrigX = coord.X(); + const Standard_Real aOrigY = coord.Y(); + const Standard_Real aOrigZ = coord.Z(); + + // Normalize the mirror vector components + const Standard_Real aNormDirX = aMirrorVecXYZ.X() / aMagnitude; + const Standard_Real aNormDirY = aMirrorVecXYZ.Y() / aMagnitude; + const Standard_Real aNormDirZ = aMirrorVecXYZ.Z() / aMagnitude; + + // Precompute common terms for 3D reflection matrix + const Standard_Real aCrossTermXY = 2.0 * aNormDirX * aNormDirY; + const Standard_Real aCrossTermXZ = 2.0 * aNormDirX * aNormDirZ; + const Standard_Real aCrossTermYZ = 2.0 * aNormDirY * aNormDirZ; + const Standard_Real aXXTerm = 2.0 * aNormDirX * aNormDirX - 1.0; + const Standard_Real aYYTerm = 2.0 * aNormDirY * aNormDirY - 1.0; + const Standard_Real aZZTerm = 2.0 * aNormDirZ * aNormDirZ - 1.0; + + coord.SetX(aXXTerm * aOrigX + aCrossTermXY * aOrigY + aCrossTermXZ * aOrigZ); + coord.SetY(aCrossTermXY * aOrigX + aYYTerm * aOrigY + aCrossTermYZ * aOrigZ); + coord.SetZ(aCrossTermXZ * aOrigX + aCrossTermYZ * aOrigY + aZZTerm * aOrigZ); } } -void gp_Vec::Mirror(const gp_Ax1& A1) +void gp_Vec::Mirror(const gp_Ax1& theAxis) { - const gp_XYZ& V = A1.Direction().XYZ(); - Standard_Real A = V.X(); - Standard_Real B = V.Y(); - Standard_Real C = V.Z(); - Standard_Real X = coord.X(); - Standard_Real Y = coord.Y(); - Standard_Real Z = coord.Z(); - Standard_Real M1 = 2.0 * A * B; - Standard_Real M2 = 2.0 * A * C; - Standard_Real M3 = 2.0 * B * C; - coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z); - coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z); - coord.SetZ(M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z); + const gp_XYZ& aDirectionXYZ = theAxis.Direction().XYZ(); + const Standard_Real aOrigX = coord.X(); + const Standard_Real aOrigY = coord.Y(); + const Standard_Real aOrigZ = coord.Z(); + const Standard_Real aDirX = aDirectionXYZ.X(); + const Standard_Real aDirY = aDirectionXYZ.Y(); + const Standard_Real aDirZ = aDirectionXYZ.Z(); + + // Precompute common terms for 3D reflection matrix + const Standard_Real aCrossTermXY = 2.0 * aDirX * aDirY; + const Standard_Real aCrossTermXZ = 2.0 * aDirX * aDirZ; + const Standard_Real aCrossTermYZ = 2.0 * aDirY * aDirZ; + const Standard_Real aXXTerm = 2.0 * aDirX * aDirX - 1.0; + const Standard_Real aYYTerm = 2.0 * aDirY * aDirY - 1.0; + const Standard_Real aZZTerm = 2.0 * aDirZ * aDirZ - 1.0; + + coord.SetX(aXXTerm * aOrigX + aCrossTermXY * aOrigY + aCrossTermXZ * aOrigZ); + coord.SetY(aCrossTermXY * aOrigX + aYYTerm * aOrigY + aCrossTermYZ * aOrigZ); + coord.SetZ(aCrossTermXZ * aOrigX + aCrossTermYZ * aOrigY + aZZTerm * aOrigZ); } -void gp_Vec::Mirror(const gp_Ax2& A2) +void gp_Vec::Mirror(const gp_Ax2& theAxis) { - gp_XYZ Z = A2.Direction().XYZ(); - gp_XYZ MirXYZ = Z.Crossed(coord); - if (MirXYZ.Modulus() <= gp::Resolution()) + const gp_XYZ& aZDir = theAxis.Direction().XYZ(); + const gp_XYZ aMirXYZ = aZDir.Crossed(coord); + + if (aMirXYZ.Modulus() <= gp::Resolution()) { coord.Reverse(); } else { - Z.Cross(MirXYZ); - Mirror(Z); + gp_XYZ aNewZ = aZDir; + aNewZ.Cross(aMirXYZ); + Mirror(gp_Vec(aNewZ)); } } -void gp_Vec::Transform(const gp_Trsf& T) +void gp_Vec::Transform(const gp_Trsf& theTransformation) { - if (T.Form() == gp_Identity || T.Form() == gp_Translation) + if (theTransformation.Form() == gp_Identity || theTransformation.Form() == gp_Translation) { } - else if (T.Form() == gp_PntMirror) + else if (theTransformation.Form() == gp_PntMirror) { coord.Reverse(); } - else if (T.Form() == gp_Scale) + else if (theTransformation.Form() == gp_Scale) { - coord.Multiply(T.ScaleFactor()); + coord.Multiply(theTransformation.ScaleFactor()); } else { - coord.Multiply(T.VectorialPart()); + coord.Multiply(theTransformation.VectorialPart()); } } -gp_Vec gp_Vec::Mirrored(const gp_Vec& V) const +gp_Vec gp_Vec::Mirrored(const gp_Vec& theVec) const { - gp_Vec Vres = *this; - Vres.Mirror(V); - return Vres; + gp_Vec aResult = *this; + aResult.Mirror(theVec); + return aResult; } -gp_Vec gp_Vec::Mirrored(const gp_Ax1& A1) const +gp_Vec gp_Vec::Mirrored(const gp_Ax1& theAxis) const { - gp_Vec Vres = *this; - Vres.Mirror(A1); - return Vres; + gp_Vec aResult = *this; + aResult.Mirror(theAxis); + return aResult; } -gp_Vec gp_Vec::Mirrored(const gp_Ax2& A2) const +gp_Vec gp_Vec::Mirrored(const gp_Ax2& theAxis) const { - gp_Vec Vres = *this; - Vres.Mirror(A2); - return Vres; + gp_Vec aResult = *this; + aResult.Mirror(theAxis); + return aResult; } //================================================================================================= diff --git a/src/FoundationClasses/TKMath/gp/gp_Vec.hxx b/src/FoundationClasses/TKMath/gp/gp_Vec.hxx index 1d30b81a5d..5df6876913 100644 --- a/src/FoundationClasses/TKMath/gp/gp_Vec.hxx +++ b/src/FoundationClasses/TKMath/gp/gp_Vec.hxx @@ -129,7 +129,7 @@ public: //! Other.Magnitude() <= Resolution from gp Standard_Boolean IsOpposite(const gp_Vec& theOther, const Standard_Real theAngularTolerance) const { - Standard_Real anAng = M_PI - Angle(theOther); + const Standard_Real anAng = M_PI - Angle(theOther); return anAng <= theAngularTolerance; } @@ -141,7 +141,7 @@ public: //! Other.Magnitude() <= Resolution from gp Standard_Boolean IsParallel(const gp_Vec& theOther, const Standard_Real theAngularTolerance) const { - Standard_Real anAng = Angle(theOther); + const Standard_Real anAng = Angle(theOther); return anAng <= theAngularTolerance || M_PI - anAng <= theAngularTolerance; } @@ -303,7 +303,7 @@ public: //! lower or equal to Resolution from gp. void Normalize() { - Standard_Real aD = coord.Modulus(); + const Standard_Real aD = coord.Modulus(); Standard_ConstructionError_Raise_if(aD <= gp::Resolution(), "gp_Vec::Normalize() - vector has zero norm"); coord.Divide(aD); @@ -474,11 +474,7 @@ inline gp_Vec::gp_Vec(const gp_Pnt& theP1, const gp_Pnt& theP2) inline Standard_Boolean gp_Vec::IsNormal(const gp_Vec& theOther, const Standard_Real theAngularTolerance) const { - Standard_Real anAng = M_PI / 2.0 - Angle(theOther); - if (anAng < 0) - { - anAng = -anAng; - } + const Standard_Real anAng = Abs(M_PI_2 - Angle(theOther)); return anAng <= theAngularTolerance; } @@ -513,7 +509,7 @@ inline Standard_Real gp_Vec::AngleWithRef(const gp_Vec& theOther, const gp_Vec& //======================================================================= inline gp_Vec gp_Vec::Normalized() const { - Standard_Real aD = coord.Modulus(); + const Standard_Real aD = coord.Modulus(); Standard_ConstructionError_Raise_if(aD <= gp::Resolution(), "gp_Vec::Normalized() - vector has zero norm"); gp_Vec aV = *this; diff --git a/src/FoundationClasses/TKMath/gp/gp_Vec2d.cxx b/src/FoundationClasses/TKMath/gp/gp_Vec2d.cxx index 76d8fe4fc0..1a9a99a6c6 100644 --- a/src/FoundationClasses/TKMath/gp/gp_Vec2d.cxx +++ b/src/FoundationClasses/TKMath/gp/gp_Vec2d.cxx @@ -25,117 +25,131 @@ #include #include -Standard_Boolean gp_Vec2d::IsEqual(const gp_Vec2d& Other, - const Standard_Real LinearTolerance, - const Standard_Real AngularTolerance) const +Standard_Boolean gp_Vec2d::IsEqual(const gp_Vec2d& theOther, + const Standard_Real theLinearTolerance, + const Standard_Real theAngularTolerance) const { - const Standard_Real theNorm = Magnitude(); - const Standard_Real theOtherNorm = Other.Magnitude(); - Standard_Real val = theNorm - theOtherNorm; - if (val < 0.0) - val = -val; + const Standard_Real aNorm = Magnitude(); + const Standard_Real anOtherNorm = theOther.Magnitude(); + const Standard_Real aVal = Abs(aNorm - anOtherNorm); // Check for equal lengths - const Standard_Boolean isEqualLength = (val <= LinearTolerance); + const Standard_Boolean isEqualLength = (aVal <= theLinearTolerance); // Check for small vectors - if (theNorm > LinearTolerance && theOtherNorm > LinearTolerance) + if (aNorm > theLinearTolerance && anOtherNorm > theLinearTolerance) { - Standard_Real Ang = Angle(Other); - if (Ang < 0.0) - Ang = -Ang; + const Standard_Real anAng = Abs(Angle(theOther)); // Check for zero angle - return isEqualLength && (Ang <= AngularTolerance); + return isEqualLength && (anAng <= theAngularTolerance); } return isEqualLength; } -Standard_Real gp_Vec2d::Angle(const gp_Vec2d& Other) const +Standard_Real gp_Vec2d::Angle(const gp_Vec2d& theOther) const { - // Commentaires : - // Au dessus de 45 degres l'arccos donne la meilleur precision pour le - // calcul de l'angle. Sinon il vaut mieux utiliser l'arcsin. - // Les erreurs commises sont loin d'etre negligeables lorsque l'on est - // proche de zero ou de 90 degres. - // En 2D les valeurs angulaires sont comprises entre -PI et PI - const Standard_Real theNorm = Magnitude(); - const Standard_Real theOtherNorm = Other.Magnitude(); - if (theNorm <= gp::Resolution() || theOtherNorm <= gp::Resolution()) + // Comments: + // Above 45 degrees arccos gives the best precision for the + // angle calculation. Otherwise it is better to use arcsin. + // The errors made are far from negligible when we are + // close to zero or 90 degrees. + // In 2D the angular values are between -PI and PI + const Standard_Real aNorm = Magnitude(); + const Standard_Real anOtherNorm = theOther.Magnitude(); + if (aNorm <= gp::Resolution() || anOtherNorm <= gp::Resolution()) throw gp_VectorWithNullMagnitude(); - const Standard_Real D = theNorm * theOtherNorm; - const Standard_Real Cosinus = coord.Dot(Other.coord) / D; - const Standard_Real Sinus = coord.Crossed(Other.coord) / D; - if (Cosinus > -0.70710678118655 && Cosinus < 0.70710678118655) + const Standard_Real aD = aNorm * anOtherNorm; + const Standard_Real aCosinus = coord.Dot(theOther.coord) / aD; + const Standard_Real aSinus = coord.Crossed(theOther.coord) / aD; + + // Use M_SQRT1_2 (1/sqrt(2) approximately 0.7071067811865476) for better readability and precision + constexpr Standard_Real aCOS_45_DEG = M_SQRT1_2; + + if (aCosinus > -aCOS_45_DEG && aCosinus < aCOS_45_DEG) { - if (Sinus > 0.0) - return acos(Cosinus); - else - return -acos(Cosinus); + // For angles near +/-90 degrees, use acos for better precision + return (aSinus > 0.0) ? acos(aCosinus) : -acos(aCosinus); } else { - if (Cosinus > 0.0) - return asin(Sinus); + // For angles near 0 degrees or +/-180 degrees, use asin for better precision + if (aCosinus > 0.0) + return asin(aSinus); else - { - if (Sinus > 0.0) - return M_PI - asin(Sinus); - else - return -M_PI - asin(Sinus); - } + return (aSinus > 0.0) ? M_PI - asin(aSinus) : -M_PI - asin(aSinus); } } -void gp_Vec2d::Mirror(const gp_Ax2d& A1) +void gp_Vec2d::Mirror(const gp_Ax2d& theA1) { - const gp_XY& XY = A1.Direction().XY(); - Standard_Real X = coord.X(); - Standard_Real Y = coord.Y(); - Standard_Real A = XY.X(); - Standard_Real B = XY.Y(); - Standard_Real M1 = 2.0 * A * B; - coord.SetX(((2.0 * A * A) - 1.) * X + M1 * Y); - coord.SetY(M1 * X + ((2. * B * B) - 1.0) * Y); + const gp_XY& aDirectionXY = theA1.Direction().XY(); + const Standard_Real aOrigX = coord.X(); + const Standard_Real aOrigY = coord.Y(); + const Standard_Real aDirX = aDirectionXY.X(); + const Standard_Real aDirY = aDirectionXY.Y(); + + // Precompute common terms for reflection matrix + const Standard_Real aCrossTerm = 2.0 * aDirX * aDirY; + const Standard_Real aXXTerm = 2.0 * aDirX * aDirX - 1.0; + const Standard_Real aYYTerm = 2.0 * aDirY * aDirY - 1.0; + + coord.SetX(aXXTerm * aOrigX + aCrossTerm * aOrigY); + coord.SetY(aCrossTerm * aOrigX + aYYTerm * aOrigY); } -gp_Vec2d gp_Vec2d::Mirrored(const gp_Ax2d& A1) const +gp_Vec2d gp_Vec2d::Mirrored(const gp_Ax2d& theA1) const { gp_Vec2d Vres = *this; - Vres.Mirror(A1); + Vres.Mirror(theA1); return Vres; } -void gp_Vec2d::Transform(const gp_Trsf2d& T) +void gp_Vec2d::Transform(const gp_Trsf2d& theT) { - if (T.Form() == gp_Identity || T.Form() == gp_Translation) + switch (theT.Form()) { - } - else if (T.Form() == gp_PntMirror) - coord.Reverse(); - else if (T.Form() == gp_Scale) - coord.Multiply(T.ScaleFactor()); - else - coord.Multiply(T.VectorialPart()); -} + case gp_Identity: + case gp_Translation: + break; -void gp_Vec2d::Mirror(const gp_Vec2d& V) -{ - const Standard_Real D = V.coord.Modulus(); - if (D > gp::Resolution()) - { - const gp_XY& XY = V.coord; - Standard_Real X = XY.X(); - Standard_Real Y = XY.Y(); - Standard_Real A = X / D; - Standard_Real B = Y / D; - Standard_Real M1 = 2.0 * A * B; - coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y); - coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y); + case gp_PntMirror: + coord.Reverse(); + break; + + case gp_Scale: + coord.Multiply(theT.ScaleFactor()); + break; + + default: + coord.Multiply(theT.VectorialPart()); } } -gp_Vec2d gp_Vec2d::Mirrored(const gp_Vec2d& V) const +void gp_Vec2d::Mirror(const gp_Vec2d& theV) { - gp_Vec2d Vres = *this; - Vres.Mirror(V); - return Vres; + const Standard_Real aMagnitude = theV.coord.Modulus(); + if (aMagnitude > gp::Resolution()) + { + const gp_XY& aMirrorVecXY = theV.coord; + const Standard_Real aOrigX = coord.X(); + const Standard_Real aOrigY = coord.Y(); + + // Normalize the mirror vector components + const Standard_Real aNormDirX = aMirrorVecXY.X() / aMagnitude; + const Standard_Real aNormDirY = aMirrorVecXY.Y() / aMagnitude; + + // Precompute common terms for reflection matrix + const Standard_Real aCrossTerm = 2.0 * aNormDirX * aNormDirY; + const Standard_Real aXXTerm = 2.0 * aNormDirX * aNormDirX - 1.0; + const Standard_Real aYYTerm = 2.0 * aNormDirY * aNormDirY - 1.0; + + coord.SetX(aXXTerm * aOrigX + aCrossTerm * aOrigY); + coord.SetY(aCrossTerm * aOrigX + aYYTerm * aOrigY); + } +} + +gp_Vec2d gp_Vec2d::Mirrored(const gp_Vec2d& theV) const +{ + gp_Vec2d aVres = *this; + aVres.Mirror(theV); + return aVres; } diff --git a/src/FoundationClasses/TKMath/gp/gp_Vec2d.hxx b/src/FoundationClasses/TKMath/gp/gp_Vec2d.hxx index de36a7e579..f8a4eea485 100644 --- a/src/FoundationClasses/TKMath/gp/gp_Vec2d.hxx +++ b/src/FoundationClasses/TKMath/gp/gp_Vec2d.hxx @@ -382,11 +382,7 @@ inline gp_Vec2d::gp_Vec2d(const gp_Pnt2d& theP1, const gp_Pnt2d& theP2) inline Standard_Boolean gp_Vec2d::IsOpposite(const gp_Vec2d& theOther, const Standard_Real theAngularTolerance) const { - Standard_Real anAng = Angle(theOther); - if (anAng < 0) - { - anAng = -anAng; - } + const Standard_Real anAng = Abs(Angle(theOther)); return M_PI - anAng <= theAngularTolerance; } @@ -397,11 +393,7 @@ inline Standard_Boolean gp_Vec2d::IsOpposite(const gp_Vec2d& theOther, inline Standard_Boolean gp_Vec2d::IsParallel(const gp_Vec2d& theOther, const Standard_Real theAngularTolerance) const { - Standard_Real anAng = Angle(theOther); - if (anAng < 0) - { - anAng = -anAng; - } + const Standard_Real anAng = Abs(Angle(theOther)); return anAng <= theAngularTolerance || M_PI - anAng <= theAngularTolerance; } diff --git a/src/FoundationClasses/TKMath/gp/gp_XY.cxx b/src/FoundationClasses/TKMath/gp/gp_XY.cxx deleted file mode 100644 index cad7d27f78..0000000000 --- a/src/FoundationClasses/TKMath/gp/gp_XY.cxx +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright (c) 1995-1999 Matra Datavision -// Copyright (c) 1999-2014 OPEN CASCADE SAS -// -// This file is part of Open CASCADE Technology software library. -// -// This library is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License version 2.1 as published -// by the Free Software Foundation, with special exception defined in the file -// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT -// distribution for complete text of the license and disclaimer of any warranty. -// -// Alternatively, this file may be used under the terms of Open CASCADE -// commercial license or contractual agreement. - -#include - -#include - -Standard_Boolean gp_XY::IsEqual(const gp_XY& Other, const Standard_Real Tolerance) const -{ - Standard_Real val; - val = x - Other.x; - if (val < 0) - val = -val; - if (val > Tolerance) - return Standard_False; - val = y - Other.y; - if (val < 0) - val = -val; - if (val > Tolerance) - return Standard_False; - return Standard_True; -} diff --git a/src/FoundationClasses/TKMath/gp/gp_XY.hxx b/src/FoundationClasses/TKMath/gp/gp_XY.hxx index 8aa7d0a7ab..3c514c8630 100644 --- a/src/FoundationClasses/TKMath/gp/gp_XY.hxx +++ b/src/FoundationClasses/TKMath/gp/gp_XY.hxx @@ -99,19 +99,18 @@ public: Standard_Real Y() const { return y; } //! Computes Sqrt (X*X + Y*Y) where X and Y are the two coordinates of this number pair. - Standard_Real Modulus() const { return sqrt(x * x + y * y); } + Standard_Real Modulus() const { return sqrt(SquareModulus()); } //! Computes X*X + Y*Y where X and Y are the two coordinates of this number pair. Standard_Real SquareModulus() const { return x * x + y * y; } //! Returns true if the coordinates of this number pair are //! equal to the respective coordinates of the number pair - //! theOther, within the specified tolerance theTolerance. I.e.: - //! abs(.X() - theOther.X()) <= theTolerance and - //! abs(.Y() - theOther.Y()) <= theTolerance and - //! computations - Standard_EXPORT Standard_Boolean IsEqual(const gp_XY& theOther, - const Standard_Real theTolerance) const; + //! theOther, within the specified tolerance theTolerance. + Standard_Boolean IsEqual(const gp_XY& theOther, const Standard_Real theTolerance) const + { + return (Abs(x - theOther.x) < theTolerance) && (Abs(y - theOther.y) < theTolerance); + } //! Computes the sum of this number pair and number pair theOther //! @code @@ -155,15 +154,14 @@ public: //! theRight. Returns || ^ theRight || inline Standard_Real CrossMagnitude(const gp_XY& theRight) const { - Standard_Real aVal = x * theRight.y - y * theRight.x; - return aVal < 0 ? -aVal : aVal; + return Abs(x * theRight.y - y * theRight.x); } //! computes the square magnitude of the cross product between and //! theRight. Returns || ^ theRight ||**2 inline Standard_Real CrossSquareMagnitude(const gp_XY& theRight) const { - Standard_Real aZresult = x * theRight.y - y * theRight.x; + const Standard_Real aZresult = x * theRight.y - y * theRight.x; return aZresult * aZresult; } @@ -247,8 +245,8 @@ public: //! New = theMatrix * Standard_NODISCARD gp_XY Multiplied(const gp_Mat2d& theMatrix) const { - return gp_XY(theMatrix.Value(1, 1) * x + theMatrix.Value(1, 2) * y, - theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y); + return gp_XY(theMatrix.myMat[0][0] * x + theMatrix.myMat[0][1] * y, + theMatrix.myMat[1][0] * x + theMatrix.myMat[1][1] * y); } Standard_NODISCARD gp_XY operator*(const gp_Mat2d& theMatrix) const @@ -385,9 +383,9 @@ private: //======================================================================= inline void gp_XY::Multiply(const gp_Mat2d& theMatrix) { - Standard_Real aXresult = theMatrix.Value(1, 1) * x + theMatrix.Value(1, 2) * y; - y = theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y; - x = aXresult; + const Standard_Real aXresult = theMatrix.myMat[0][0] * x + theMatrix.myMat[0][1] * y; + y = theMatrix.myMat[1][0] * x + theMatrix.myMat[1][1] * y; + x = aXresult; } //======================================================================= diff --git a/src/FoundationClasses/TKMath/gp/gp_XYZ.cxx b/src/FoundationClasses/TKMath/gp/gp_XYZ.cxx index 59b875b53a..ffc5ccb46d 100644 --- a/src/FoundationClasses/TKMath/gp/gp_XYZ.cxx +++ b/src/FoundationClasses/TKMath/gp/gp_XYZ.cxx @@ -15,31 +15,8 @@ #include #include -#include -#include #include -Standard_Boolean gp_XYZ::IsEqual(const gp_XYZ& Other, const Standard_Real Tolerance) const -{ - Standard_Real val; - val = x - Other.x; - if (val < 0) - val = -val; - if (val > Tolerance) - return Standard_False; - val = y - Other.y; - if (val < 0) - val = -val; - if (val > Tolerance) - return Standard_False; - val = z - Other.z; - if (val < 0) - val = -val; - if (val > Tolerance) - return Standard_False; - return Standard_True; -} - //================================================================================================= void gp_XYZ::DumpJson(Standard_OStream& theOStream, Standard_Integer) const { diff --git a/src/FoundationClasses/TKMath/gp/gp_XYZ.hxx b/src/FoundationClasses/TKMath/gp/gp_XYZ.hxx index e39752275f..f4c05f2b45 100644 --- a/src/FoundationClasses/TKMath/gp/gp_XYZ.hxx +++ b/src/FoundationClasses/TKMath/gp/gp_XYZ.hxx @@ -130,12 +130,13 @@ public: //! Returns True if he coordinates of this XYZ object are //! equal to the respective coordinates Other, - //! within the specified tolerance theTolerance. I.e.: - //! abs(.X() - theOther.X()) <= theTolerance and - //! abs(.Y() - theOther.Y()) <= theTolerance and - //! abs(.Z() - theOther.Z()) <= theTolerance. - Standard_EXPORT Standard_Boolean IsEqual(const gp_XYZ& theOther, - const Standard_Real theTolerance) const; + //! within the specified tolerance theTolerance. + Standard_Boolean IsEqual(const gp_XYZ& theOther, const Standard_Real theTolerance) const + + { + return (Abs(x - theOther.x) < theTolerance) && (Abs(y - theOther.y) < theTolerance) + && (Abs(z - theOther.z) < theTolerance); + } //! @code //! .X() = .X() + theOther.X() @@ -300,10 +301,11 @@ public: //! New = theMatrix * Standard_NODISCARD gp_XYZ Multiplied(const gp_Mat& theMatrix) const { - return gp_XYZ(theMatrix.Value(1, 1) * x + theMatrix.Value(1, 2) * y + theMatrix.Value(1, 3) * z, - theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y + theMatrix.Value(2, 3) * z, - theMatrix.Value(3, 1) * x + theMatrix.Value(3, 2) * y - + theMatrix.Value(3, 3) * z); + // Direct access to matrix data for optimal performance (gp_XYZ is friend of gp_Mat) + return gp_XYZ(theMatrix.myMat[0][0] * x + theMatrix.myMat[0][1] * y + theMatrix.myMat[0][2] * z, + theMatrix.myMat[1][0] * x + theMatrix.myMat[1][1] * y + theMatrix.myMat[1][2] * z, + theMatrix.myMat[2][0] * x + theMatrix.myMat[2][1] * y + + theMatrix.myMat[2][2] * z); } Standard_NODISCARD gp_XYZ operator*(const gp_Mat& theMatrix) const @@ -327,7 +329,7 @@ public: //! Raised if .Modulus() <= Resolution from gp Standard_NODISCARD gp_XYZ Normalized() const { - Standard_Real aD = Modulus(); + const Standard_Real aD = Modulus(); Standard_ConstructionError_Raise_if(aD <= gp::Resolution(), "gp_XYZ::Normalized() - vector has zero norm"); return gp_XYZ(x / aD, y / aD, z / aD); @@ -481,11 +483,11 @@ private: //======================================================================= inline void gp_XYZ::Cross(const gp_XYZ& theRight) { - Standard_Real aXresult = y * theRight.z - z * theRight.y; - Standard_Real aYresult = z * theRight.x - x * theRight.z; - z = x * theRight.y - y * theRight.x; - x = aXresult; - y = aYresult; + const Standard_Real aXresult = y * theRight.z - z * theRight.y; + const Standard_Real aYresult = z * theRight.x - x * theRight.z; + z = x * theRight.y - y * theRight.x; + x = aXresult; + y = aYresult; } //======================================================================= @@ -494,10 +496,7 @@ inline void gp_XYZ::Cross(const gp_XYZ& theRight) //======================================================================= inline Standard_Real gp_XYZ::CrossMagnitude(const gp_XYZ& theRight) const { - Standard_Real aXresult = y * theRight.z - z * theRight.y; - Standard_Real aYresult = z * theRight.x - x * theRight.z; - Standard_Real aZresult = x * theRight.y - y * theRight.x; - return sqrt(aXresult * aXresult + aYresult * aYresult + aZresult * aZresult); + return sqrt(CrossSquareMagnitude(theRight)); } //======================================================================= @@ -506,9 +505,9 @@ inline Standard_Real gp_XYZ::CrossMagnitude(const gp_XYZ& theRight) const //======================================================================= inline Standard_Real gp_XYZ::CrossSquareMagnitude(const gp_XYZ& theRight) const { - Standard_Real aXresult = y * theRight.z - z * theRight.y; - Standard_Real aYresult = z * theRight.x - x * theRight.z; - Standard_Real aZresult = x * theRight.y - y * theRight.x; + const Standard_Real aXresult = y * theRight.z - z * theRight.y; + const Standard_Real aYresult = z * theRight.x - x * theRight.z; + const Standard_Real aZresult = x * theRight.y - y * theRight.x; return aXresult * aXresult + aYresult * aYresult + aZresult * aZresult; } @@ -518,14 +517,18 @@ inline Standard_Real gp_XYZ::CrossSquareMagnitude(const gp_XYZ& theRight) const //======================================================================= inline void gp_XYZ::CrossCross(const gp_XYZ& theCoord1, const gp_XYZ& theCoord2) { - Standard_Real aXresult = y * (theCoord1.x * theCoord2.y - theCoord1.y * theCoord2.x) - - z * (theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z); - Standard_Real anYresult = z * (theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y) - - x * (theCoord1.x * theCoord2.y - theCoord1.y * theCoord2.x); - z = x * (theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z) - - y * (theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y); + // First compute theCoord1 * theCoord2 + const Standard_Real aCrossX = theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y; + const Standard_Real aCrossY = theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z; + const Standard_Real aCrossZ = theCoord1.x * theCoord2.y - theCoord1.y * theCoord2.x; + + // Then compute this * (theCoord1 * theCoord2) + const Standard_Real aXresult = y * aCrossZ - z * aCrossY; + const Standard_Real aYresult = z * aCrossX - x * aCrossZ; + + z = x * aCrossY - y * aCrossX; x = aXresult; - y = anYresult; + y = aYresult; } //======================================================================= @@ -534,9 +537,9 @@ inline void gp_XYZ::CrossCross(const gp_XYZ& theCoord1, const gp_XYZ& theCoord2) //======================================================================= inline Standard_Real gp_XYZ::DotCross(const gp_XYZ& theCoord1, const gp_XYZ& theCoord2) const { - Standard_Real aXresult = theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y; - Standard_Real anYresult = theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z; - Standard_Real aZresult = theCoord1.x * theCoord2.y - theCoord1.y * theCoord2.x; + const Standard_Real aXresult = theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y; + const Standard_Real anYresult = theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z; + const Standard_Real aZresult = theCoord1.x * theCoord2.y - theCoord1.y * theCoord2.x; return (x * aXresult + y * anYresult + z * aZresult); } @@ -546,13 +549,18 @@ inline Standard_Real gp_XYZ::DotCross(const gp_XYZ& theCoord1, const gp_XYZ& the //======================================================================= inline void gp_XYZ::Multiply(const gp_Mat& theMatrix) { - Standard_Real aXresult = - theMatrix.Value(1, 1) * x + theMatrix.Value(1, 2) * y + theMatrix.Value(1, 3) * z; - Standard_Real anYresult = - theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y + theMatrix.Value(2, 3) * z; - z = theMatrix.Value(3, 1) * x + theMatrix.Value(3, 2) * y + theMatrix.Value(3, 3) * z; - x = aXresult; - y = anYresult; + // Cache original coordinates to avoid aliasing issues + const Standard_Real aOrigX = x; + const Standard_Real aOrigY = y; + const Standard_Real aOrigZ = z; + + // Matrix-vector multiplication: this = theMatrix * this + x = theMatrix.myMat[0][0] * aOrigX + theMatrix.myMat[0][1] * aOrigY + + theMatrix.myMat[0][2] * aOrigZ; + y = theMatrix.myMat[1][0] * aOrigX + theMatrix.myMat[1][1] * aOrigY + + theMatrix.myMat[1][2] * aOrigZ; + z = theMatrix.myMat[2][0] * aOrigX + theMatrix.myMat[2][1] * aOrigY + + theMatrix.myMat[2][2] * aOrigZ; } //=======================================================================