OpenCascade/occt
1
0
Fork 0
mirror of https://git.dev.opencascade.org/repos/occt.git synced 2025-06-30 12:14:08 +03:00
Code

Coding - Optimize gp_Vec, gp_Vec2d, gp_XY, and gp_XYZ classes (#578)

Browse Source 
Renamed parameters and improved consistency across methods.
Simplified mathematical computations and replaced indirect API calls with direct data member access where performance‐critical.
Updated and optimized matrix operations including inversion, transposition, and power calculations.
This commit is contained in:
Pasukhin Dmitry 2025-06-26 09:37:35 +01:00 committed by GitHub
parent ed7a447177
commit 6b69f59803
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 380 additions and 455 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/FoundationClasses/TKMath/gp/FILES.cmake
View File

@ -78,7 +78,6 @@ set(OCCT_gp_FILES
gp_Vec2f.hxx gp_Vec2f.hxx
gp_Vec3f.hxx gp_Vec3f.hxx
gp_VectorWithNullMagnitude.hxx gp_VectorWithNullMagnitude.hxx
gp_XY.cxx
gp_XY.hxx gp_XY.hxx
gp_XYZ.cxx gp_XYZ.cxx
gp_XYZ.hxx gp_XYZ.hxx

204
src/FoundationClasses/TKMath/gp/gp_Mat.cxx
View File

@ -88,12 +88,13 @@ void gp_Mat::SetCross(const gp_XYZ& theRef)
const Standard_Real Y = theRef.Y(); const Standard_Real Y = theRef.Y();
const Standard_Real Z = theRef.Z(); const Standard_Real Z = theRef.Z();
myMat[0][0] = myMat[1][1] = myMat[2][2] = 0.0; myMat[0][0] = myMat[1][1] = myMat[2][2] = 0.0;
myMat[0][1] = -Z;
myMat[0][2] = Y; myMat[0][1] = -Z;
myMat[1][2] = -X; myMat[0][2] = Y;
myMat[1][0] = Z; myMat[1][2] = -X;
myMat[2][0] = -Y; myMat[1][0] = Z;
myMat[2][1] = X; 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 X = theRef.X();
const Standard_Real Y = theRef.Y(); const Standard_Real Y = theRef.Y();
const Standard_Real Z = theRef.Z(); const Standard_Real Z = theRef.Z();
myMat[0][0] = X * X;
myMat[1][1] = Y * Y; myMat[0][0] = X * X;
myMat[2][2] = Z * Z; myMat[1][1] = Y * Y;
myMat[0][1] = X * Y; myMat[2][2] = Z * Z;
myMat[0][2] = X * Z; myMat[0][1] = X * Y;
myMat[1][2] = Y * Z; myMat[0][2] = X * Z;
myMat[1][0] = myMat[0][1]; myMat[1][2] = Y * Z;
myMat[2][0] = myMat[0][2];
myMat[2][1] = myMat[1][2]; 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) void gp_Mat::SetRotation(const gp_XYZ& theAxis, const Standard_Real theAng)
{ {
// Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M // Rodrigues' rotation formula: R = I + sin(θ)K + (1-cos(θ))K²
// avec M . XYZ = Axis ^ XYZ // Where K is the skew-symmetric matrix of the normalized axis
const gp_XYZ aV = theAxis.Normalized(); 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 A = aV.X();
const Standard_Real B = aV.Y(); const Standard_Real B = aV.Y();
const Standard_Real C = aV.Z(); 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)); // Precompute trigonometric values
aTemp.SetRow(3, gp_XYZ(A * C, B * C, -A * A - B * B)); const Standard_Real aCos = cos(theAng);
aTemp.Multiply(1.0 - cos(theAng)); const Standard_Real aSin = sin(theAng);
Add(aTemp); 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() void gp_Mat::Invert()
{ {
Standard_Real aNewMat[3][3]; // Optimized matrix inversion using cached elements
// calcul de la transposee de la commatrice const Standard_Real a00 = myMat[0][0], a01 = myMat[0][1], a02 = myMat[0][2];
aNewMat[0][0] = myMat[1][1] * myMat[2][2] - myMat[1][2] * myMat[2][1]; const Standard_Real a10 = myMat[1][0], a11 = myMat[1][1], a12 = myMat[1][2];
aNewMat[1][0] = -(myMat[1][0] * myMat[2][2] - myMat[2][0] * myMat[1][2]); const Standard_Real a20 = myMat[2][0], a21 = myMat[2][1], a22 = myMat[2][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]); // Compute adjugate matrix (transpose of cofactor matrix)
aNewMat[1][1] = myMat[0][0] * myMat[2][2] - myMat[2][0] * myMat[0][2]; const Standard_Real adj00 = a11 * a22 - a12 * a21;
aNewMat[2][1] = -(myMat[0][0] * myMat[2][1] - myMat[2][0] * myMat[0][1]); const Standard_Real adj10 = a12 * a20 - a10 * a22;
aNewMat[0][2] = myMat[0][1] * myMat[1][2] - myMat[1][1] * myMat[0][2]; const Standard_Real adj20 = a10 * a21 - a11 * a20;
aNewMat[1][2] = -(myMat[0][0] * myMat[1][2] - myMat[1][0] * myMat[0][2]); const Standard_Real adj01 = a02 * a21 - a01 * a22;
aNewMat[2][2] = myMat[0][0] * myMat[1][1] - myMat[0][1] * myMat[1][0]; const Standard_Real adj11 = a00 * a22 - a02 * a20;
Standard_Real aDet = const Standard_Real adj21 = a01 * a20 - a00 * a21;
myMat[0][0] * aNewMat[0][0] + myMat[0][1] * aNewMat[1][0] + myMat[0][2] * aNewMat[2][0]; 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(), Standard_ConstructionError_Raise_if(Abs(aDet) <= gp::Resolution(),
"gp_Mat::Invert() - matrix has zero determinant"); "gp_Mat::Invert() - matrix has zero determinant");
aDet = 1.0e0 / aDet;
myMat[0][0] = aNewMat[0][0]; // Compute inverse: inv(A) = adj(A) / det(A)
myMat[1][0] = aNewMat[1][0]; const Standard_Real aInvDet = 1.0 / aDet;
myMat[2][0] = aNewMat[2][0];
myMat[0][1] = aNewMat[0][1]; // Direct assignment with scaling
myMat[1][1] = aNewMat[1][1]; myMat[0][0] = adj00 * aInvDet;
myMat[2][1] = aNewMat[2][1]; myMat[1][0] = adj10 * aInvDet;
myMat[0][2] = aNewMat[0][2]; myMat[2][0] = adj20 * aInvDet;
myMat[1][2] = aNewMat[1][2]; myMat[0][1] = adj01 * aInvDet;
myMat[2][2] = aNewMat[2][2]; myMat[1][1] = adj11 * aInvDet;
Multiply(aDet); 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 gp_Mat::Inverted() const
{ {
gp_Mat aNewMat; gp_Mat aNewMat(*this);
// calcul de la transposee de la commatrice aNewMat.Invert();
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);
return aNewMat; return aNewMat;
} }
@ -267,37 +283,45 @@ gp_Mat gp_Mat::Inverted() const
void gp_Mat::Power(const Standard_Integer theN) void gp_Mat::Power(const Standard_Integer theN)
{ {
if (theN == 1) // Optimized matrix exponentiation using fast binary exponentiation
{ if (theN == 0)
}
else if (theN == 0)
{ {
SetIdentity(); 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(); 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(); Multiply(aBase);
}
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;
} }
aBase.Multiply(aBase); // Square the base
power >>= 1; // Move to next bit
} }
} }

79
src/FoundationClasses/TKMath/gp/gp_Mat.hxx
View File

@ -141,9 +141,14 @@ public:
//! Computes the determinant of the matrix. //! Computes the determinant of the matrix.
Standard_Real Determinant() const Standard_Real Determinant() const
{ {
return myMat[0][0] * (myMat[1][1] * myMat[2][2] - myMat[2][1] * myMat[1][2]) const Standard_Real a00 = myMat[0][0], a01 = myMat[0][1], a02 = myMat[0][2];
- myMat[0][1] * (myMat[1][0] * myMat[2][2] - myMat[2][0] * myMat[1][2]) const Standard_Real a10 = myMat[1][0], a11 = myMat[1][1], a12 = myMat[1][2];
+ myMat[0][2] * (myMat[1][0] * myMat[2][1] - myMat[2][0] * myMat[1][1]); 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. //! 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 inline gp_Mat gp_Mat::Added(const gp_Mat& theOther) const
{ {
gp_Mat aNewMat; gp_Mat aNewMat(*this);
aNewMat.myMat[0][0] = myMat[0][0] + theOther.myMat[0][0]; aNewMat.Add(theOther);
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];
return aNewMat; 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 inline gp_Mat gp_Mat::Divided(const Standard_Real theScalar) const
{ {
Standard_Real aVal = theScalar; gp_Mat aNewMat(*this);
if (aVal < 0) aNewMat.Divide(theScalar);
{
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;
return aNewMat; 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 inline gp_Mat gp_Mat::Multiplied(const Standard_Real theScalar) const
{ {
gp_Mat aNewMat; gp_Mat aNewMat(*this);
aNewMat.myMat[0][0] = theScalar * myMat[0][0]; aNewMat.Multiply(theScalar);
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];
return aNewMat; 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 inline gp_Mat gp_Mat::Subtracted(const gp_Mat& theOther) const
{ {
gp_Mat aNewMat; gp_Mat aNewMat(*this);
aNewMat.myMat[0][0] = myMat[0][0] - theOther.myMat[0][0]; aNewMat.Subtract(theOther);
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];
return aNewMat; return aNewMat;
} }
@ -573,16 +539,9 @@ __attribute__((optnone))
inline void inline void
gp_Mat::Transpose() gp_Mat::Transpose()
{ {
Standard_Real aTemp; std::swap(myMat[0][1], myMat[1][0]);
aTemp = myMat[0][1]; std::swap(myMat[0][2], myMat[2][0]);
myMat[0][1] = myMat[1][0]; std::swap(myMat[1][2], myMat[2][1]);
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;
} }
//======================================================================= //=======================================================================

36
src/FoundationClasses/TKMath/gp/gp_Pnt.hxx
View File

@ -259,21 +259,7 @@ struct equal_to<gp_Pnt>
//======================================================================= //=======================================================================
inline Standard_Real gp_Pnt::Distance(const gp_Pnt& theOther) const inline Standard_Real gp_Pnt::Distance(const gp_Pnt& theOther) const
{ {
Standard_Real aD = 0, aDD; return sqrt(SquareDistance(theOther));
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);
} }
//======================================================================= //=======================================================================
@ -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 inline Standard_Real gp_Pnt::SquareDistance(const gp_Pnt& theOther) const
{ {
Standard_Real aD = 0, aDD; const gp_XYZ& aXYZ = theOther.coord;
const gp_XYZ& XYZ = theOther.coord; const Standard_Real aDx = coord.X() - aXYZ.X();
aDD = coord.X(); const Standard_Real aDy = coord.Y() - aXYZ.Y();
aDD -= XYZ.X(); const Standard_Real aDz = coord.Z() - aXYZ.Z();
aDD *= aDD; return aDx * aDx + aDy * aDy + aDz * aDz;
aD += aDD;
aDD = coord.Y();
aDD -= XYZ.Y();
aDD *= aDD;
aD += aDD;
aDD = coord.Z();
aDD -= XYZ.Z();
aDD *= aDD;
aD += aDD;
return aD;
} }
//======================================================================= //=======================================================================

147
src/FoundationClasses/TKMath/gp/gp_Vec.cxx
View File

@ -29,117 +29,132 @@
#include <Standard_Dump.hxx> #include <Standard_Dump.hxx>
#include <Standard_OutOfRange.hxx> #include <Standard_OutOfRange.hxx>
Standard_Boolean gp_Vec::IsEqual(const gp_Vec& Other, Standard_Boolean gp_Vec::IsEqual(const gp_Vec& theOther,
const Standard_Real LinearTolerance, const Standard_Real theLinearTolerance,
const Standard_Real AngularTolerance) const 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(); const Standard_Real aVal = Abs(aMagnitude - anOtherMagnitude);
if (val < 0) return aVal <= theLinearTolerance;
val = -val;
return val <= LinearTolerance;
} }
else else
{ {
Standard_Real val = Magnitude() - Other.Magnitude(); const Standard_Real aVal = Abs(aMagnitude - anOtherMagnitude);
if (val < 0) return aVal <= theLinearTolerance && Angle(theOther) <= theAngularTolerance;
val = -val;
return val <= LinearTolerance && Angle(Other) <= AngularTolerance;
} }
} }
void gp_Vec::Mirror(const gp_Vec& V) void gp_Vec::Mirror(const gp_Vec& theVec)
{ {
Standard_Real D = V.coord.Modulus(); const Standard_Real aMagnitude = theVec.coord.Modulus();
if (D > gp::Resolution()) if (aMagnitude > gp::Resolution())
{ {
const gp_XYZ& XYZ = V.coord; const gp_XYZ& aMirrorVecXYZ = theVec.coord;
Standard_Real A = XYZ.X() / D; const Standard_Real aOrigX = coord.X();
Standard_Real B = XYZ.Y() / D; const Standard_Real aOrigY = coord.Y();
Standard_Real C = XYZ.Z() / D; const Standard_Real aOrigZ = coord.Z();
Standard_Real M1 = 2.0 * A * B;
Standard_Real M2 = 2.0 * A * C; // Normalize the mirror vector components
Standard_Real M3 = 2.0 * B * C; const Standard_Real aNormDirX = aMirrorVecXYZ.X() / aMagnitude;
Standard_Real X = coord.X(); const Standard_Real aNormDirY = aMirrorVecXYZ.Y() / aMagnitude;
Standard_Real Y = coord.Y(); const Standard_Real aNormDirZ = aMirrorVecXYZ.Z() / aMagnitude;
Standard_Real Z = coord.Z();
coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z); // Precompute common terms for 3D reflection matrix
coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z); const Standard_Real aCrossTermXY = 2.0 * aNormDirX * aNormDirY;
coord.SetZ(M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z); 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(); const gp_XYZ& aDirectionXYZ = theAxis.Direction().XYZ();
Standard_Real A = V.X(); const Standard_Real aOrigX = coord.X();
Standard_Real B = V.Y(); const Standard_Real aOrigY = coord.Y();
Standard_Real C = V.Z(); const Standard_Real aOrigZ = coord.Z();
Standard_Real X = coord.X(); const Standard_Real aDirX = aDirectionXYZ.X();
Standard_Real Y = coord.Y(); const Standard_Real aDirY = aDirectionXYZ.Y();
Standard_Real Z = coord.Z(); const Standard_Real aDirZ = aDirectionXYZ.Z();
Standard_Real M1 = 2.0 * A * B;
Standard_Real M2 = 2.0 * A * C; // Precompute common terms for 3D reflection matrix
Standard_Real M3 = 2.0 * B * C; const Standard_Real aCrossTermXY = 2.0 * aDirX * aDirY;
coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z); const Standard_Real aCrossTermXZ = 2.0 * aDirX * aDirZ;
coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z); const Standard_Real aCrossTermYZ = 2.0 * aDirY * aDirZ;
coord.SetZ(M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z); 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(); const gp_XYZ& aZDir = theAxis.Direction().XYZ();
gp_XYZ MirXYZ = Z.Crossed(coord); const gp_XYZ aMirXYZ = aZDir.Crossed(coord);
if (MirXYZ.Modulus() <= gp::Resolution())
if (aMirXYZ.Modulus() <= gp::Resolution())
{ {
coord.Reverse(); coord.Reverse();
} }
else else
{ {
Z.Cross(MirXYZ); gp_XYZ aNewZ = aZDir;
Mirror(Z); 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(); coord.Reverse();
} }
else if (T.Form() == gp_Scale) else if (theTransformation.Form() == gp_Scale)
{ {
coord.Multiply(T.ScaleFactor()); coord.Multiply(theTransformation.ScaleFactor());
} }
else 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; gp_Vec aResult = *this;
Vres.Mirror(V); aResult.Mirror(theVec);
return Vres; 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; gp_Vec aResult = *this;
Vres.Mirror(A1); aResult.Mirror(theAxis);
return Vres; 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; gp_Vec aResult = *this;
Vres.Mirror(A2); aResult.Mirror(theAxis);
return Vres; return aResult;
} }
//================================================================================================= //=================================================================================================

14
src/FoundationClasses/TKMath/gp/gp_Vec.hxx
View File

@ -129,7 +129,7 @@ public:
//! Other.Magnitude() <= Resolution from gp //! Other.Magnitude() <= Resolution from gp
Standard_Boolean IsOpposite(const gp_Vec& theOther, const Standard_Real theAngularTolerance) const 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; return anAng <= theAngularTolerance;
} }
@ -141,7 +141,7 @@ public:
//! Other.Magnitude() <= Resolution from gp //! Other.Magnitude() <= Resolution from gp
Standard_Boolean IsParallel(const gp_Vec& theOther, const Standard_Real theAngularTolerance) const 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; return anAng <= theAngularTolerance || M_PI - anAng <= theAngularTolerance;
} }
@ -303,7 +303,7 @@ public:
//! lower or equal to Resolution from gp. //! lower or equal to Resolution from gp.
void Normalize() void Normalize()
{ {
Standard_Real aD = coord.Modulus(); const Standard_Real aD = coord.Modulus();
Standard_ConstructionError_Raise_if(aD <= gp::Resolution(), Standard_ConstructionError_Raise_if(aD <= gp::Resolution(),
"gp_Vec::Normalize() - vector has zero norm"); "gp_Vec::Normalize() - vector has zero norm");
coord.Divide(aD); 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, inline Standard_Boolean gp_Vec::IsNormal(const gp_Vec& theOther,
const Standard_Real theAngularTolerance) const const Standard_Real theAngularTolerance) const
{ {
Standard_Real anAng = M_PI / 2.0 - Angle(theOther); const Standard_Real anAng = Abs(M_PI_2 - Angle(theOther));
if (anAng < 0)
{
anAng = -anAng;
}
return anAng <= theAngularTolerance; 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 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(), Standard_ConstructionError_Raise_if(aD <= gp::Resolution(),
"gp_Vec::Normalized() - vector has zero norm"); "gp_Vec::Normalized() - vector has zero norm");
gp_Vec aV = *this; gp_Vec aV = *this;

170
src/FoundationClasses/TKMath/gp/gp_Vec2d.cxx
View File

@ -25,117 +25,131 @@
#include <gp_VectorWithNullMagnitude.hxx> #include <gp_VectorWithNullMagnitude.hxx>
#include <gp_XY.hxx> #include <gp_XY.hxx>
Standard_Boolean gp_Vec2d::IsEqual(const gp_Vec2d& Other, Standard_Boolean gp_Vec2d::IsEqual(const gp_Vec2d& theOther,
const Standard_Real LinearTolerance, const Standard_Real theLinearTolerance,
const Standard_Real AngularTolerance) const const Standard_Real theAngularTolerance) const
{ {
const Standard_Real theNorm = Magnitude(); const Standard_Real aNorm = Magnitude();
const Standard_Real theOtherNorm = Other.Magnitude(); const Standard_Real anOtherNorm = theOther.Magnitude();
Standard_Real val = theNorm - theOtherNorm; const Standard_Real aVal = Abs(aNorm - anOtherNorm);
if (val < 0.0)
val = -val;
// Check for equal lengths // Check for equal lengths
const Standard_Boolean isEqualLength = (val <= LinearTolerance); const Standard_Boolean isEqualLength = (aVal <= theLinearTolerance);
// Check for small vectors // Check for small vectors
if (theNorm > LinearTolerance && theOtherNorm > LinearTolerance) if (aNorm > theLinearTolerance && anOtherNorm > theLinearTolerance)
{ {
Standard_Real Ang = Angle(Other); const Standard_Real anAng = Abs(Angle(theOther));
if (Ang < 0.0)
Ang = -Ang;
// Check for zero angle // Check for zero angle
return isEqualLength && (Ang <= AngularTolerance); return isEqualLength && (anAng <= theAngularTolerance);
} }
return isEqualLength; return isEqualLength;
} }
Standard_Real gp_Vec2d::Angle(const gp_Vec2d& Other) const Standard_Real gp_Vec2d::Angle(const gp_Vec2d& theOther) const
{ {
// Commentaires : // Comments:
// Au dessus de 45 degres l'arccos donne la meilleur precision pour le // Above 45 degrees arccos gives the best precision for the
// calcul de l'angle. Sinon il vaut mieux utiliser l'arcsin. // angle calculation. Otherwise it is better to use arcsin.
// Les erreurs commises sont loin d'etre negligeables lorsque l'on est // The errors made are far from negligible when we are
// proche de zero ou de 90 degres. // close to zero or 90 degrees.
// En 2D les valeurs angulaires sont comprises entre -PI et PI // In 2D the angular values are between -PI and PI
const Standard_Real theNorm = Magnitude(); const Standard_Real aNorm = Magnitude();
const Standard_Real theOtherNorm = Other.Magnitude(); const Standard_Real anOtherNorm = theOther.Magnitude();
if (theNorm <= gp::Resolution() || theOtherNorm <= gp::Resolution()) if (aNorm <= gp::Resolution() || anOtherNorm <= gp::Resolution())
throw gp_VectorWithNullMagnitude(); throw gp_VectorWithNullMagnitude();
const Standard_Real D = theNorm * theOtherNorm; const Standard_Real aD = aNorm * anOtherNorm;
const Standard_Real Cosinus = coord.Dot(Other.coord) / D; const Standard_Real aCosinus = coord.Dot(theOther.coord) / aD;
const Standard_Real Sinus = coord.Crossed(Other.coord) / D; const Standard_Real aSinus = coord.Crossed(theOther.coord) / aD;
if (Cosinus > -0.70710678118655 && Cosinus < 0.70710678118655)
// 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) // For angles near +/-90 degrees, use acos for better precision
return acos(Cosinus); return (aSinus > 0.0) ? acos(aCosinus) : -acos(aCosinus);
else
return -acos(Cosinus);
} }
else else
{ {
if (Cosinus > 0.0) // For angles near 0 degrees or +/-180 degrees, use asin for better precision
return asin(Sinus); if (aCosinus > 0.0)
return asin(aSinus);
else else
{ return (aSinus > 0.0) ? M_PI - asin(aSinus) : -M_PI - asin(aSinus);
if (Sinus > 0.0)
return M_PI - asin(Sinus);
else
return -M_PI - asin(Sinus);
}
} }
} }
void gp_Vec2d::Mirror(const gp_Ax2d& A1) void gp_Vec2d::Mirror(const gp_Ax2d& theA1)
{ {
const gp_XY& XY = A1.Direction().XY(); const gp_XY& aDirectionXY = theA1.Direction().XY();
Standard_Real X = coord.X(); const Standard_Real aOrigX = coord.X();
Standard_Real Y = coord.Y(); const Standard_Real aOrigY = coord.Y();
Standard_Real A = XY.X(); const Standard_Real aDirX = aDirectionXY.X();
Standard_Real B = XY.Y(); const Standard_Real aDirY = aDirectionXY.Y();
Standard_Real M1 = 2.0 * A * B;
coord.SetX(((2.0 * A * A) - 1.) * X + M1 * Y); // Precompute common terms for reflection matrix
coord.SetY(M1 * X + ((2. * B * B) - 1.0) * Y); 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; gp_Vec2d Vres = *this;
Vres.Mirror(A1); Vres.Mirror(theA1);
return Vres; 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())
{ {
} case gp_Identity:
else if (T.Form() == gp_PntMirror) case gp_Translation:
coord.Reverse(); break;
else if (T.Form() == gp_Scale)
coord.Multiply(T.ScaleFactor());
else
coord.Multiply(T.VectorialPart());
}
void gp_Vec2d::Mirror(const gp_Vec2d& V) case gp_PntMirror:
{ coord.Reverse();
const Standard_Real D = V.coord.Modulus(); break;
if (D > gp::Resolution())
{ case gp_Scale:
const gp_XY& XY = V.coord; coord.Multiply(theT.ScaleFactor());
Standard_Real X = XY.X(); break;
Standard_Real Y = XY.Y();
Standard_Real A = X / D; default:
Standard_Real B = Y / D; coord.Multiply(theT.VectorialPart());
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);
} }
} }
gp_Vec2d gp_Vec2d::Mirrored(const gp_Vec2d& V) const void gp_Vec2d::Mirror(const gp_Vec2d& theV)
{ {
gp_Vec2d Vres = *this; const Standard_Real aMagnitude = theV.coord.Modulus();
Vres.Mirror(V); if (aMagnitude > gp::Resolution())
return Vres; {
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;
} }

12
src/FoundationClasses/TKMath/gp/gp_Vec2d.hxx
View File

@ -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, inline Standard_Boolean gp_Vec2d::IsOpposite(const gp_Vec2d& theOther,
const Standard_Real theAngularTolerance) const const Standard_Real theAngularTolerance) const
{ {
Standard_Real anAng = Angle(theOther); const Standard_Real anAng = Abs(Angle(theOther));
if (anAng < 0)
{
anAng = -anAng;
}
return M_PI - anAng <= theAngularTolerance; 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, inline Standard_Boolean gp_Vec2d::IsParallel(const gp_Vec2d& theOther,
const Standard_Real theAngularTolerance) const const Standard_Real theAngularTolerance) const
{ {
Standard_Real anAng = Angle(theOther); const Standard_Real anAng = Abs(Angle(theOther));
if (anAng < 0)
{
anAng = -anAng;
}
return anAng <= theAngularTolerance || M_PI - anAng <= theAngularTolerance; return anAng <= theAngularTolerance || M_PI - anAng <= theAngularTolerance;
} }

33
src/FoundationClasses/TKMath/gp/gp_XY.cxx
View File

@ -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 <gp_XY.hxx>
#include <Standard_OutOfRange.hxx>
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;
}

28
src/FoundationClasses/TKMath/gp/gp_XY.hxx
View File

@ -99,19 +99,18 @@ public:
Standard_Real Y() const { return y; } Standard_Real Y() const { return y; }
//! Computes Sqrt (X*X + Y*Y) where X and Y are the two coordinates of this number pair. //! 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. //! 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; } Standard_Real SquareModulus() const { return x * x + y * y; }
//! Returns true if the coordinates of this number pair are //! Returns true if the coordinates of this number pair are
//! equal to the respective coordinates of the number pair //! equal to the respective coordinates of the number pair
//! theOther, within the specified tolerance theTolerance. I.e.: //! theOther, within the specified tolerance theTolerance.
//! abs(<me>.X() - theOther.X()) <= theTolerance and Standard_Boolean IsEqual(const gp_XY& theOther, const Standard_Real theTolerance) const
//! abs(<me>.Y() - theOther.Y()) <= theTolerance and {
//! computations return (Abs(x - theOther.x) < theTolerance) && (Abs(y - theOther.y) < theTolerance);
Standard_EXPORT Standard_Boolean IsEqual(const gp_XY& theOther, }
const Standard_Real theTolerance) const;
//! Computes the sum of this number pair and number pair theOther //! Computes the sum of this number pair and number pair theOther
//! @code //! @code
@ -155,15 +154,14 @@ public:
//! theRight. Returns || <me> ^ theRight || //! theRight. Returns || <me> ^ theRight ||
inline Standard_Real CrossMagnitude(const gp_XY& theRight) const inline Standard_Real CrossMagnitude(const gp_XY& theRight) const
{ {
Standard_Real aVal = x * theRight.y - y * theRight.x; return Abs(x * theRight.y - y * theRight.x);
return aVal < 0 ? -aVal : aVal;
} }
//! computes the square magnitude of the cross product between <me> and //! computes the square magnitude of the cross product between <me> and
//! theRight. Returns || <me> ^ theRight ||**2 //! theRight. Returns || <me> ^ theRight ||**2
inline Standard_Real CrossSquareMagnitude(const gp_XY& theRight) const 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; return aZresult * aZresult;
} }
@ -247,8 +245,8 @@ public:
//! New = theMatrix * <me> //! New = theMatrix * <me>
Standard_NODISCARD gp_XY Multiplied(const gp_Mat2d& theMatrix) const Standard_NODISCARD gp_XY Multiplied(const gp_Mat2d& theMatrix) const
{ {
return gp_XY(theMatrix.Value(1, 1) * x + theMatrix.Value(1, 2) * y, return gp_XY(theMatrix.myMat[0][0] * x + theMatrix.myMat[0][1] * y,
theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y); theMatrix.myMat[1][0] * x + theMatrix.myMat[1][1] * y);
} }
Standard_NODISCARD gp_XY operator*(const gp_Mat2d& theMatrix) const Standard_NODISCARD gp_XY operator*(const gp_Mat2d& theMatrix) const
@ -385,9 +383,9 @@ private:
//======================================================================= //=======================================================================
inline void gp_XY::Multiply(const gp_Mat2d& theMatrix) inline void gp_XY::Multiply(const gp_Mat2d& theMatrix)
{ {
Standard_Real aXresult = theMatrix.Value(1, 1) * x + theMatrix.Value(1, 2) * y; const Standard_Real aXresult = theMatrix.myMat[0][0] * x + theMatrix.myMat[0][1] * y;
y = theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y; y = theMatrix.myMat[1][0] * x + theMatrix.myMat[1][1] * y;
x = aXresult; x = aXresult;
} }
//======================================================================= //=======================================================================

23
src/FoundationClasses/TKMath/gp/gp_XYZ.cxx
View File

@ -15,31 +15,8 @@
#include <gp_XYZ.hxx> #include <gp_XYZ.hxx>
#include <gp_Mat.hxx> #include <gp_Mat.hxx>
#include <Standard_ConstructionError.hxx>
#include <Standard_OutOfRange.hxx>
#include <Standard_Dump.hxx> #include <Standard_Dump.hxx>
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 { void gp_XYZ::DumpJson(Standard_OStream& theOStream, Standard_Integer) const {

88
src/FoundationClasses/TKMath/gp/gp_XYZ.hxx
View File

@ -130,12 +130,13 @@ public:
//! Returns True if he coordinates of this XYZ object are //! Returns True if he coordinates of this XYZ object are
//! equal to the respective coordinates Other, //! equal to the respective coordinates Other,
//! within the specified tolerance theTolerance. I.e.: //! within the specified tolerance theTolerance.
//! abs(<me>.X() - theOther.X()) <= theTolerance and Standard_Boolean IsEqual(const gp_XYZ& theOther, const Standard_Real theTolerance) const
//! abs(<me>.Y() - theOther.Y()) <= theTolerance and
//! abs(<me>.Z() - theOther.Z()) <= theTolerance. {
Standard_EXPORT Standard_Boolean IsEqual(const gp_XYZ& theOther, return (Abs(x - theOther.x) < theTolerance) && (Abs(y - theOther.y) < theTolerance)
const Standard_Real theTolerance) const; && (Abs(z - theOther.z) < theTolerance);
}
//! @code //! @code
//! <me>.X() = <me>.X() + theOther.X() //! <me>.X() = <me>.X() + theOther.X()
@ -300,10 +301,11 @@ public:
//! New = theMatrix * <me> //! New = theMatrix * <me>
Standard_NODISCARD gp_XYZ Multiplied(const gp_Mat& theMatrix) const 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, // Direct access to matrix data for optimal performance (gp_XYZ is friend of gp_Mat)
theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y + theMatrix.Value(2, 3) * z, return gp_XYZ(theMatrix.myMat[0][0] * x + theMatrix.myMat[0][1] * y + theMatrix.myMat[0][2] * z,
theMatrix.Value(3, 1) * x + theMatrix.Value(3, 2) * y theMatrix.myMat[1][0] * x + theMatrix.myMat[1][1] * y + theMatrix.myMat[1][2] * z,
+ theMatrix.Value(3, 3) * 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 Standard_NODISCARD gp_XYZ operator*(const gp_Mat& theMatrix) const
@ -327,7 +329,7 @@ public:
//! Raised if <me>.Modulus() <= Resolution from gp //! Raised if <me>.Modulus() <= Resolution from gp
Standard_NODISCARD gp_XYZ Normalized() const Standard_NODISCARD gp_XYZ Normalized() const
{ {
Standard_Real aD = Modulus(); const Standard_Real aD = Modulus();
Standard_ConstructionError_Raise_if(aD <= gp::Resolution(), Standard_ConstructionError_Raise_if(aD <= gp::Resolution(),
"gp_XYZ::Normalized() - vector has zero norm"); "gp_XYZ::Normalized() - vector has zero norm");
return gp_XYZ(x / aD, y / aD, z / aD); return gp_XYZ(x / aD, y / aD, z / aD);
@ -481,11 +483,11 @@ private:
//======================================================================= //=======================================================================
inline void gp_XYZ::Cross(const gp_XYZ& theRight) inline void gp_XYZ::Cross(const gp_XYZ& theRight)
{ {
Standard_Real aXresult = y * theRight.z - z * theRight.y; const Standard_Real aXresult = y * theRight.z - z * theRight.y;
Standard_Real aYresult = z * theRight.x - x * theRight.z; const Standard_Real aYresult = z * theRight.x - x * theRight.z;
z = x * theRight.y - y * theRight.x; z = x * theRight.y - y * theRight.x;
x = aXresult; x = aXresult;
y = aYresult; 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 inline Standard_Real gp_XYZ::CrossMagnitude(const gp_XYZ& theRight) const
{ {
Standard_Real aXresult = y * theRight.z - z * theRight.y; return sqrt(CrossSquareMagnitude(theRight));
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);
} }
//======================================================================= //=======================================================================
@ -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 inline Standard_Real gp_XYZ::CrossSquareMagnitude(const gp_XYZ& theRight) const
{ {
Standard_Real aXresult = y * theRight.z - z * theRight.y; const Standard_Real aXresult = y * theRight.z - z * theRight.y;
Standard_Real aYresult = z * theRight.x - x * theRight.z; const Standard_Real aYresult = z * theRight.x - x * theRight.z;
Standard_Real aZresult = x * theRight.y - y * theRight.x; const Standard_Real aZresult = x * theRight.y - y * theRight.x;
return aXresult * aXresult + aYresult * aYresult + aZresult * aZresult; 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) 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) // First compute theCoord1 * theCoord2
- z * (theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z); const Standard_Real aCrossX = theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y;
Standard_Real anYresult = z * (theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y) const Standard_Real aCrossY = theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z;
- x * (theCoord1.x * theCoord2.y - theCoord1.y * theCoord2.x); const Standard_Real aCrossZ = 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); // 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; 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 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; const Standard_Real aXresult = theCoord1.y * theCoord2.z - theCoord1.z * theCoord2.y;
Standard_Real anYresult = theCoord1.z * theCoord2.x - theCoord1.x * theCoord2.z; const 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 aZresult = theCoord1.x * theCoord2.y - theCoord1.y * theCoord2.x;
return (x * aXresult + y * anYresult + z * aZresult); 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) inline void gp_XYZ::Multiply(const gp_Mat& theMatrix)
{ {
Standard_Real aXresult = // Cache original coordinates to avoid aliasing issues
theMatrix.Value(1, 1) * x + theMatrix.Value(1, 2) * y + theMatrix.Value(1, 3) * z; const Standard_Real aOrigX = x;
Standard_Real anYresult = const Standard_Real aOrigY = y;
theMatrix.Value(2, 1) * x + theMatrix.Value(2, 2) * y + theMatrix.Value(2, 3) * z; const Standard_Real aOrigZ = z;
z = theMatrix.Value(3, 1) * x + theMatrix.Value(3, 2) * y + theMatrix.Value(3, 3) * z;
x = aXresult; // Matrix-vector multiplication: this = theMatrix * this
y = anYresult; 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;
} }
//======================================================================= //=======================================================================