mirror of
https://git.dev.opencascade.org/repos/occt.git
synced 2025-06-30 12:14:08 +03:00
Coding - Optimize gp_Vec, gp_Vec2d, gp_XY, and gp_XYZ classes (#578)
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
GitHub
parent
ed7a447177
commit
6b69f59803
@ -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
|
||||
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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]);
|
||||
}
|
||||
|
||||
//=======================================================================
|
||||
|
||||
@ -259,21 +259,7 @@ struct equal_to<gp_Pnt>
|
||||
//=======================================================================
|
||||
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;
|
||||
}
|
||||
|
||||
//=======================================================================
|
||||
|
||||
@ -29,117 +29,132 @@
|
||||
#include <Standard_Dump.hxx>
|
||||
#include <Standard_OutOfRange.hxx>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
//=================================================================================================
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -25,117 +25,131 @@
|
||||
#include <gp_VectorWithNullMagnitude.hxx>
|
||||
#include <gp_XY.hxx>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -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(<me>.X() - theOther.X()) <= theTolerance and
|
||||
//! abs(<me>.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 || <me> ^ 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 <me> and
|
||||
//! theRight. Returns || <me> ^ 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 * <me>
|
||||
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;
|
||||
}
|
||||
|
||||
//=======================================================================
|
||||
|
||||
@ -15,31 +15,8 @@
|
||||
#include <gp_XYZ.hxx>
|
||||
|
||||
#include <gp_Mat.hxx>
|
||||
#include <Standard_ConstructionError.hxx>
|
||||
#include <Standard_OutOfRange.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 {
|
||||
|
||||
@ -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(<me>.X() - theOther.X()) <= theTolerance and
|
||||
//! abs(<me>.Y() - theOther.Y()) <= theTolerance and
|
||||
//! abs(<me>.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
|
||||
//! <me>.X() = <me>.X() + theOther.X()
|
||||
@ -300,10 +301,11 @@ public:
|
||||
//! New = theMatrix * <me>
|
||||
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 <me>.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;
|
||||
}
|
||||
|
||||
//=======================================================================
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user