admin/opensim-libs
1
0
Fork 0
mirror of git://opensimulator.org/git/opensim-libs synced 2026-08-14 09:02:27 +00:00
Code Issues Packages Projects Releases Wiki Activity

cosmetics and changes on ice matrixes

Browse Source
This commit is contained in:
UbitUmarov
2022-07-31 00:50:38 +01:00
parent e829d862ec
commit ba10fc7e91
3 changed files with 147 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix3x3.h
+143 -122
View File
@@ -15,104 +15,102 @@
// Forward declarations
class Quat;
#define MATRIX3X3_EPSILON (1.0e-7f)
#define MATRIX3X3_EPSILON (1.0e-7f)
class ICEMATHS_API Matrix3x3
{
public:
//! Empty constructor
inline_ Matrix3x3() {}
inline_ Matrix3x3() {}
//! Constructor from 9 values
inline_ Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
inline_ Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
{
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
}
//! Copy constructor
inline_ Matrix3x3(const Matrix3x3& mat) { CopyMemory(m, &mat.m, 9 * sizeof(float)); }
inline_ Matrix3x3(const Matrix3x3& mat) { CopyMemory(m, &mat.m, 9 * sizeof(float)); }
//! Destructor
inline_ ~Matrix3x3() {}
inline_ ~Matrix3x3() {}
//! Assign values
inline_ void Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
inline_ void Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
{
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
}
//! Sets the scale from a Point. The point is put on the diagonal.
inline_ void SetScale(const Point& p) { m[0][0] = p.x; m[1][1] = p.y; m[2][2] = p.z; }
inline_ void SetScale(const Point& p) { m[0][0] = p.x; m[1][1] = p.y; m[2][2] = p.z; }
//! Sets the scale from floats. Values are put on the diagonal.
inline_ void SetScale(float sx, float sy, float sz) { m[0][0] = sx; m[1][1] = sy; m[2][2] = sz; }
inline_ void SetScale(float sx, float sy, float sz) { m[0][0] = sx; m[1][1] = sy; m[2][2] = sz; }
//! Scales from a Point. Each row is multiplied by a component.
inline_ void Scale(const Point& p)
inline_ void Scale(const Point& p)
{
m[0][0] *= p.x; m[0][1] *= p.x; m[0][2] *= p.x;
m[1][0] *= p.y; m[1][1] *= p.y; m[1][2] *= p.y;
m[2][0] *= p.z; m[2][1] *= p.z; m[2][2] *= p.z;
m[0][0] *= p.x; m[0][1] *= p.x; m[0][2] *= p.x;
m[1][0] *= p.y; m[1][1] *= p.y; m[1][2] *= p.y;
m[2][0] *= p.z; m[2][1] *= p.z; m[2][2] *= p.z;
}
//! Scales from floats. Each row is multiplied by a value.
inline_ void Scale(float sx, float sy, float sz)
inline_ void Scale(float sx, float sy, float sz)
{
m[0][0] *= sx; m[0][1] *= sx; m[0][2] *= sx;
m[1][0] *= sy; m[1][1] *= sy; m[1][2] *= sy;
m[2][0] *= sz; m[2][1] *= sz; m[2][2] *= sz;
m[0][0] *= sx; m[0][1] *= sx; m[0][2] *= sx;
m[1][0] *= sy; m[1][1] *= sy; m[1][2] *= sy;
m[2][0] *= sz; m[2][1] *= sz; m[2][2] *= sz;
}
//! Copy from a Matrix3x3
inline_ void Copy(const Matrix3x3& source) { CopyMemory(m, source.m, 9 * sizeof(float)); }
inline_ void Copy(const Matrix3x3& source) { CopyMemory(m, source.m, 9 * sizeof(float)); }
// Row-column access
//! Returns a row.
inline_ void GetRow(const udword r, Point& p) const { p.x = m[r][0]; p.y = m[r][1]; p.z = m[r][2]; }
inline_ void GetRow(const udword r, Point& p) const { p.x = m[r][0]; p.y = m[r][1]; p.z = m[r][2]; }
//! Returns a row.
inline_ const Point& GetRow(const udword r) const { return *(const Point*)&m[r][0]; }
//! Returns a row.
inline_ Point& GetRow(const udword r) { return *(Point*)&m[r][0]; }
inline_ Point& GetRow(const udword r) { return *(Point*)&m[r][0]; }
//! Sets a row.
inline_ void SetRow(const udword r, const Point& p) { m[r][0] = p.x; m[r][1] = p.y; m[r][2] = p.z; }
inline_ void SetRow(const udword r, const Point& p) { m[r][0] = p.x; m[r][1] = p.y; m[r][2] = p.z; }
//! Returns a column.
inline_ void GetCol(const udword c, Point& p) const { p.x = m[0][c]; p.y = m[1][c]; p.z = m[2][c]; }
inline_ void GetCol(const udword c, Point& p) const { p.x = m[0][c]; p.y = m[1][c]; p.z = m[2][c]; }
//! Sets a column.
inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; }
inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; }
//! Computes the trace. The trace is the sum of the 3 diagonal components.
inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2]; }
inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2]; }
//! Clears the matrix.
inline_ void Zero() { ZeroMemory(&m, sizeof(m)); }
inline_ void Zero() { ZeroMemory(&m, sizeof(m)); }
//! Sets the identity matrix.
inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; }
inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; }
//! Checks for identity
inline_ bool IsIdentity() const
inline_ bool IsIdentity() const
{
if (IR(m[0][0]) != IEEE_1_0) return false;
if (IR(m[0][1]) != 0) return false;
if (IR(m[0][2]) != 0) return false;
if (m[0][0] != 1.0f) return false;
if (m[0][1] != 0) return false;
if (m[0][2] != 0) return false;
if (IR(m[1][0]) != 0) return false;
if (IR(m[1][1]) != IEEE_1_0) return false;
if (IR(m[1][2]) != 0) return false;
if (m[1][0] != 0) return false;
if (m[1][1] != 1.0f) return false;
if (m[1][2] != 0) return false;
if (IR(m[2][0]) != 0) return false;
if (IR(m[2][1]) != 0) return false;
if (IR(m[2][2]) != IEEE_1_0) return false;
if (m[2][0] != 0) return false;
if (m[2][1] != 0) return false;
if (m[2][2] != 1.0f) return false;
return true;
}
//! Checks matrix validity
inline_ BOOL IsValid() const
inline_ BOOL IsValid() const
{
for (udword j = 0; j < 3; j++)
for (int j = 0; j < 3; j++)
{
for (udword i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
if (!IsValidFloat(m[j][i])) return FALSE;
if (!IsValidFloat(m[j][i])) return FALSE;
}
}
return TRUE;
@@ -124,7 +122,7 @@ public:
//! [ -a.y a.x 0.0 ]
//! This is also called a "cross matrix" since for any vectors A and B,
//! A^B = Skew(A) * B = - B * Skew(A);
inline_ void SkewSymmetric(const Point& a)
inline_ void SkewSymmetric(const Point& a)
{
m[0][0] = 0.0f;
m[0][1] = -a.z;
@@ -140,38 +138,38 @@ public:
}
//! Negates the matrix
inline_ void Neg()
inline_ void Neg()
{
m[0][0] = -m[0][0]; m[0][1] = -m[0][1]; m[0][2] = -m[0][2];
m[1][0] = -m[1][0]; m[1][1] = -m[1][1]; m[1][2] = -m[1][2];
m[2][0] = -m[2][0]; m[2][1] = -m[2][1]; m[2][2] = -m[2][2];
m[0][0] = -m[0][0]; m[0][1] = -m[0][1]; m[0][2] = -m[0][2];
m[1][0] = -m[1][0]; m[1][1] = -m[1][1]; m[1][2] = -m[1][2];
m[2][0] = -m[2][0]; m[2][1] = -m[2][1]; m[2][2] = -m[2][2];
}
//! Neg from another matrix
inline_ void Neg(const Matrix3x3& mat)
inline_ void Neg(const Matrix3x3& mat)
{
m[0][0] = -mat.m[0][0]; m[0][1] = -mat.m[0][1]; m[0][2] = -mat.m[0][2];
m[1][0] = -mat.m[1][0]; m[1][1] = -mat.m[1][1]; m[1][2] = -mat.m[1][2];
m[2][0] = -mat.m[2][0]; m[2][1] = -mat.m[2][1]; m[2][2] = -mat.m[2][2];
m[0][0] = -mat.m[0][0]; m[0][1] = -mat.m[0][1]; m[0][2] = -mat.m[0][2];
m[1][0] = -mat.m[1][0]; m[1][1] = -mat.m[1][1]; m[1][2] = -mat.m[1][2];
m[2][0] = -mat.m[2][0]; m[2][1] = -mat.m[2][1]; m[2][2] = -mat.m[2][2];
}
//! Add another matrix
inline_ void Add(const Matrix3x3& mat)
inline_ void Add(const Matrix3x3& mat)
{
m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2];
m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2];
m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2];
m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2];
m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2];
m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2];
}
//! Sub another matrix
inline_ void Sub(const Matrix3x3& mat)
inline_ void Sub(const Matrix3x3& mat)
{
m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2];
m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2];
m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2];
m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2];
m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2];
m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2];
}
//! Mac
inline_ void Mac(const Matrix3x3& a, const Matrix3x3& b, float s)
inline_ void Mac(const Matrix3x3& a, const Matrix3x3& b, float s)
{
m[0][0] = a.m[0][0] + b.m[0][0] * s;
m[0][1] = a.m[0][1] + b.m[0][1] * s;
@@ -186,37 +184,49 @@ public:
m[2][2] = a.m[2][2] + b.m[2][2] * s;
}
//! Mac
inline_ void Mac(const Matrix3x3& a, float s)
inline_ void Mac(const Matrix3x3& a, float s)
{
m[0][0] += a.m[0][0] * s; m[0][1] += a.m[0][1] * s; m[0][2] += a.m[0][2] * s;
m[1][0] += a.m[1][0] * s; m[1][1] += a.m[1][1] * s; m[1][2] += a.m[1][2] * s;
m[2][0] += a.m[2][0] * s; m[2][1] += a.m[2][1] * s; m[2][2] += a.m[2][2] * s;
m[0][0] += a.m[0][0] * s;
m[0][1] += a.m[0][1] * s;
m[0][2] += a.m[0][2] * s;
m[1][0] += a.m[1][0] * s;
m[1][1] += a.m[1][1] * s;
m[1][2] += a.m[1][2] * s;
m[2][0] += a.m[2][0] * s;
m[2][1] += a.m[2][1] * s;
m[2][2] += a.m[2][2] * s;
}
//! this = A * s
inline_ void Mult(const Matrix3x3& a, float s)
inline_ void Mult(const Matrix3x3& a, float s)
{
m[0][0] = a.m[0][0] * s; m[0][1] = a.m[0][1] * s; m[0][2] = a.m[0][2] * s;
m[1][0] = a.m[1][0] * s; m[1][1] = a.m[1][1] * s; m[1][2] = a.m[1][2] * s;
m[2][0] = a.m[2][0] * s; m[2][1] = a.m[2][1] * s; m[2][2] = a.m[2][2] * s;
m[0][0] = a.m[0][0] * s;
m[0][1] = a.m[0][1] * s;
m[0][2] = a.m[0][2] * s;
m[1][0] = a.m[1][0] * s;
m[1][1] = a.m[1][1] * s;
m[1][2] = a.m[1][2] * s;
m[2][0] = a.m[2][0] * s;
m[2][1] = a.m[2][1] * s;
m[2][2] = a.m[2][2] * s;
}
inline_ void Add(const Matrix3x3& a, const Matrix3x3& b)
inline_ void Add(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] + b.m[0][0]; m[0][1] = a.m[0][1] + b.m[0][1]; m[0][2] = a.m[0][2] + b.m[0][2];
m[1][0] = a.m[1][0] + b.m[1][0]; m[1][1] = a.m[1][1] + b.m[1][1]; m[1][2] = a.m[1][2] + b.m[1][2];
m[2][0] = a.m[2][0] + b.m[2][0]; m[2][1] = a.m[2][1] + b.m[2][1]; m[2][2] = a.m[2][2] + b.m[2][2];
m[0][0] = a.m[0][0] + b.m[0][0]; m[0][1] = a.m[0][1] + b.m[0][1]; m[0][2] = a.m[0][2] + b.m[0][2];
m[1][0] = a.m[1][0] + b.m[1][0]; m[1][1] = a.m[1][1] + b.m[1][1]; m[1][2] = a.m[1][2] + b.m[1][2];
m[2][0] = a.m[2][0] + b.m[2][0]; m[2][1] = a.m[2][1] + b.m[2][1]; m[2][2] = a.m[2][2] + b.m[2][2];
}
inline_ void Sub(const Matrix3x3& a, const Matrix3x3& b)
inline_ void Sub(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] - b.m[0][0]; m[0][1] = a.m[0][1] - b.m[0][1]; m[0][2] = a.m[0][2] - b.m[0][2];
m[1][0] = a.m[1][0] - b.m[1][0]; m[1][1] = a.m[1][1] - b.m[1][1]; m[1][2] = a.m[1][2] - b.m[1][2];
m[2][0] = a.m[2][0] - b.m[2][0]; m[2][1] = a.m[2][1] - b.m[2][1]; m[2][2] = a.m[2][2] - b.m[2][2];
m[0][0] = a.m[0][0] - b.m[0][0]; m[0][1] = a.m[0][1] - b.m[0][1]; m[0][2] = a.m[0][2] - b.m[0][2];
m[1][0] = a.m[1][0] - b.m[1][0]; m[1][1] = a.m[1][1] - b.m[1][1]; m[1][2] = a.m[1][2] - b.m[1][2];
m[2][0] = a.m[2][0] - b.m[2][0]; m[2][1] = a.m[2][1] - b.m[2][1]; m[2][2] = a.m[2][2] - b.m[2][2];
}
//! this = a * b
inline_ void Mult(const Matrix3x3& a, const Matrix3x3& b)
inline_ void Mult(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];
@@ -230,7 +240,7 @@ public:
}
//! this = transpose(a) * b
inline_ void MultAtB(const Matrix3x3& a, const Matrix3x3& b)
inline_ void MultAtB(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0];
m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1];
@@ -244,7 +254,7 @@ public:
}
//! this = a * transpose(b)
inline_ void MultABt(const Matrix3x3& a, const Matrix3x3& b)
inline_ void MultABt(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2];
m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2];
@@ -258,13 +268,13 @@ public:
}
//! Makes a rotation matrix mapping vector "from" to vector "to".
Matrix3x3& FromTo(const Point& from, const Point& to);
Matrix3x3& FromTo(const Point& from, const Point& to);
//! Set a rotation matrix around the X axis.
//! 1 0 0
//! RX = 0 cx sx
//! 0 -sx cx
void RotX(float angle);
void RotX(float angle);
//! Set a rotation matrix around the Y axis.
//! cy 0 -sy
//! RY = 0 1 0
@@ -278,10 +288,10 @@ public:
//! cy sx.sy -sy.cx
//! RY.RX 0 cx sx
//! sy -sx.cy cx.cy
void RotYX(float y, float x);
void RotYX(float y, float x);
//! Make a rotation matrix about an arbitrary axis
Matrix3x3& Rot(float angle, const Point& axis);
Matrix3x3& Rot(float angle, const Point& axis);
#if defined(__AVX__)
void Transpose()
@@ -381,7 +391,7 @@ public:
}
*/
//! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted.
Matrix3x3& Invert()
Matrix3x3& Invert()
{
float Det = Determinant(); // Must be !=0
float OneOverDet = 1.0f / Det;
@@ -402,17 +412,17 @@ public:
return *this;
}
Matrix3x3& Normalize();
Matrix3x3& Normalize();
//! this = exp(a)
Matrix3x3& Exp(const Matrix3x3& a);
Matrix3x3& Exp(const Matrix3x3& a);
void FromQuat(const Quat &q);
void FromQuatL2(const Quat &q, float l2);
// Arithmetic operators
//! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3;
inline_ Matrix3x3 operator+(const Matrix3x3& mat) const
inline_ Matrix3x3 operator+(const Matrix3x3& mat) const
{
return Matrix3x3(
m[0][0] + mat.m[0][0], m[0][1] + mat.m[0][1], m[0][2] + mat.m[0][2],
@@ -421,7 +431,7 @@ public:
}
//! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3;
inline_ Matrix3x3 operator-(const Matrix3x3& mat) const
inline_ Matrix3x3 operator-(const Matrix3x3& mat) const
{
return Matrix3x3(
m[0][0] - mat.m[0][0], m[0][1] - mat.m[0][1], m[0][2] - mat.m[0][2],
@@ -430,7 +440,7 @@ public:
}
//! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3;
inline_ Matrix3x3 operator*(const Matrix3x3& mat) const
inline_ Matrix3x3 operator*(const Matrix3x3& mat) const
{
return Matrix3x3(
m[0][0] * mat.m[0][0] + m[0][1] * mat.m[1][0] + m[0][2] * mat.m[2][0],
@@ -447,10 +457,15 @@ public:
}
//! Operator for Point Mul = Matrix3x3 * Point;
inline_ Point operator*(const Point& v) const { return Point(GetRow(0) | v, GetRow(1) | v, GetRow(2) | v); }
inline_ Point operator*(const Point& v) const
{
return Point(m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z,
m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z,
m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z);
}
//! Operator for Matrix3x3 Mul = Matrix3x3 * float;
inline_ Matrix3x3 operator*(float s) const
inline_ Matrix3x3 operator*(float s) const
{
return Matrix3x3(
m[0][0] * s, m[0][1] * s, m[0][2] * s,
@@ -459,7 +474,7 @@ public:
}
//! Operator for Matrix3x3 Mul = float * Matrix3x3;
inline_ friend Matrix3x3 operator*(float s, const Matrix3x3& mat)
inline_ friend Matrix3x3 operator*(float s, const Matrix3x3& mat)
{
return Matrix3x3(
s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2],
@@ -478,7 +493,7 @@ public:
}
//! Operator for Matrix3x3 Div = float / Matrix3x3;
inline_ friend Matrix3x3 operator/(float s, const Matrix3x3& mat)
inline_ friend Matrix3x3 operator/(float s, const Matrix3x3& mat)
{
return Matrix3x3(
s / mat.m[0][0], s / mat.m[0][1], s / mat.m[0][2],
@@ -487,7 +502,7 @@ public:
}
//! Operator for Matrix3x3 += Matrix3x3
inline_ Matrix3x3& operator+=(const Matrix3x3& mat)
inline_ Matrix3x3& operator+=(const Matrix3x3& mat)
{
m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2];
m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2];
@@ -496,16 +511,16 @@ public:
}
//! Operator for Matrix3x3 -= Matrix3x3
inline_ Matrix3x3& operator-=(const Matrix3x3& mat)
inline_ Matrix3x3& operator-=(const Matrix3x3& mat)
{
m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2];
m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2];
m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2];
return *this;
m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2];
m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2];
m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2];
return *this;
}
//! Operator for Matrix3x3 *= Matrix3x3
inline_ Matrix3x3& operator*=(const Matrix3x3& mat)
inline_ Matrix3x3& operator*=(const Matrix3x3& mat)
{
Point TempRow;
@@ -527,36 +542,42 @@ public:
}
//! Operator for Matrix3x3 *= float
inline_ Matrix3x3& operator*=(float s)
inline_ Matrix3x3& operator*=(float s)
{
m[0][0] *= s; m[0][1] *= s; m[0][2] *= s;
m[1][0] *= s; m[1][1] *= s; m[1][2] *= s;
m[2][0] *= s; m[2][1] *= s; m[2][2] *= s;
return *this;
m[0][0] *= s; m[0][1] *= s; m[0][2] *= s;
m[1][0] *= s; m[1][1] *= s; m[1][2] *= s;
m[2][0] *= s; m[2][1] *= s; m[2][2] *= s;
return *this;
}
//! Operator for Matrix3x3 /= float
inline_ Matrix3x3& operator/=(float s)
inline_ Matrix3x3& operator/=(float s)
{
if (s) s = 1.0f / s;
m[0][0] *= s; m[0][1] *= s; m[0][2] *= s;
m[1][0] *= s; m[1][1] *= s; m[1][2] *= s;
m[2][0] *= s; m[2][1] *= s; m[2][2] *= s;
return *this;
if (s)
s = 1.0f / s;
m[0][0] *= s;
m[0][1] *= s;
m[0][2] *= s;
m[1][0] *= s;
m[1][1] *= s;
m[1][2] *= s;
m[2][0] *= s;
m[2][1] *= s;
m[2][2] *= s;
return *this;
}
// Cast operators
//! Cast a Matrix3x3 to a Matrix4x4.
operator Matrix4x4() const;
operator Matrix4x4() const;
//! Cast a Matrix3x3 to a Quat.
operator Quat() const;
operator Quat() const;
inline_ const Point& operator[](int row) const { return *(const Point*)&m[row][0]; }
inline_ Point& operator[](int row) { return *(Point*)&m[row][0]; }
inline_ const Point& operator[](int row) const { return *(const Point*)&m[row][0]; }
inline_ Point& operator[](int row) { return *(Point*)&m[row][0]; }
public:
float m[3][3];
float m[3][3];
};
#endif // __ICEMATRIX3X3_H__
trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h
+3 -1
View File
@@ -1159,7 +1159,9 @@ inline_ void iceStore3f(float *res, __m128 ma)
float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1];
float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2];
x = xp; y = yp; z = zp;
x = xp;
y = yp;
z = zp;
return *this;
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayCollider.cpp
+1 -2
View File
@@ -279,8 +279,7 @@ BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword
// The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests)
if (world)
{
Matrix3x3 InvWorld = *world;
mDir = InvWorld * world_ray.mDir;
mDir = (*world) * world_ray.mDir;
Matrix4x4 World;
InvertPRMatrix(World, *world);