mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
ab8f16961e
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
832 lines
25 KiB
C++
832 lines
25 KiB
C++
/*
|
|
*
|
|
* Mathematics Subpackage (VrMath)
|
|
*
|
|
*
|
|
* Author: Samuel R. Buss, sbuss@ucsd.edu.
|
|
* Web page: http://math.ucsd.edu/~sbuss/MathCG
|
|
*
|
|
*
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*
|
|
*
|
|
*/
|
|
|
|
#include "MathMisc.h"
|
|
#include "LinearR3.h"
|
|
#include "Spherical.h"
|
|
|
|
// ******************************************************
|
|
// * VectorR3 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
const VectorR3 UnitVecIR3(1.0, 0.0, 0.0);
|
|
const VectorR3 UnitVecJR3(0.0, 1.0, 0.0);
|
|
const VectorR3 UnitVecKR3(0.0, 0.0, 1.0);
|
|
|
|
const VectorR3 VectorR3::Zero(0.0, 0.0, 0.0);
|
|
const VectorR3 VectorR3::UnitX(1.0, 0.0, 0.0);
|
|
const VectorR3 VectorR3::UnitY(0.0, 1.0, 0.0);
|
|
const VectorR3 VectorR3::UnitZ(0.0, 0.0, 1.0);
|
|
const VectorR3 VectorR3::NegUnitX(-1.0, 0.0, 0.0);
|
|
const VectorR3 VectorR3::NegUnitY(0.0, -1.0, 0.0);
|
|
const VectorR3 VectorR3::NegUnitZ(0.0, 0.0, -1.0);
|
|
|
|
const Matrix3x3 Matrix3x3::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
|
|
const Matrix3x4 Matrix3x4::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
|
|
|
|
double VectorR3::MaxAbs() const
|
|
{
|
|
double m;
|
|
m = (x > 0.0) ? x : -x;
|
|
if (y > m)
|
|
m = y;
|
|
else if (-y > m)
|
|
m = -y;
|
|
if (z > m)
|
|
m = z;
|
|
else if (-z > m)
|
|
m = -z;
|
|
return m;
|
|
}
|
|
|
|
VectorR3& VectorR3::Set(const Quaternion& q)
|
|
{
|
|
double sinhalf = sqrt(Square(q.x) + Square(q.y) + Square(q.z));
|
|
if (sinhalf > 0.0)
|
|
{
|
|
double theta = atan2(sinhalf, q.w);
|
|
theta += theta;
|
|
this->Set(q.x, q.y, q.z);
|
|
(*this) *= (theta / sinhalf);
|
|
}
|
|
else
|
|
{
|
|
this->SetZero();
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
// *********************************************************************
|
|
// Rotation routines *
|
|
// *********************************************************************
|
|
|
|
// s.Rotate(theta, u) rotates s and returns s
|
|
// rotated theta degrees around unit vector w.
|
|
VectorR3& VectorR3::Rotate(double theta, const VectorR3& w)
|
|
{
|
|
double c = cos(theta);
|
|
double s = sin(theta);
|
|
double dotw = (x * w.x + y * w.y + z * w.z);
|
|
double v0x = dotw * w.x;
|
|
double v0y = dotw * w.y; // v0 = provjection onto w
|
|
double v0z = dotw * w.z;
|
|
double v1x = x - v0x;
|
|
double v1y = y - v0y; // v1 = projection onto plane normal to w
|
|
double v1z = z - v0z;
|
|
double v2x = w.y * v1z - w.z * v1y;
|
|
double v2y = w.z * v1x - w.x * v1z; // v2 = w * v1 (cross product)
|
|
double v2z = w.x * v1y - w.y * v1x;
|
|
|
|
x = v0x + c * v1x + s * v2x;
|
|
y = v0y + c * v1y + s * v2y;
|
|
z = v0z + c * v1z + s * v2z;
|
|
|
|
return (*this);
|
|
}
|
|
|
|
// Rotate unit vector x in the direction of "dir": length of dir is rotation angle.
|
|
// x must be a unit vector. dir must be perpindicular to x.
|
|
VectorR3& VectorR3::RotateUnitInDirection(const VectorR3& dir)
|
|
{
|
|
double theta = dir.NormSq();
|
|
if (theta == 0.0)
|
|
{
|
|
return *this;
|
|
}
|
|
else
|
|
{
|
|
theta = sqrt(theta);
|
|
double costheta = cos(theta);
|
|
double sintheta = sin(theta);
|
|
VectorR3 dirUnit = dir / theta;
|
|
*this = costheta * (*this) + sintheta * dirUnit;
|
|
return (*this);
|
|
}
|
|
}
|
|
|
|
// ******************************************************
|
|
// * Matrix3x3 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
Matrix3x3& Matrix3x3::ReNormalize() // Re-normalizes nearly orthonormal matrix
|
|
{
|
|
double alpha = m11 * m11 + m21 * m21 + m31 * m31; // First column's norm squared
|
|
double beta = m12 * m12 + m22 * m22 + m32 * m32; // Second column's norm squared
|
|
double gamma = m13 * m13 + m23 * m23 + m33 * m33; // Third column's norm squared
|
|
alpha = 1.0 - 0.5 * (alpha - 1.0); // Get mult. factor
|
|
beta = 1.0 - 0.5 * (beta - 1.0);
|
|
gamma = 1.0 - 0.5 * (gamma - 1.0);
|
|
m11 *= alpha; // Renormalize first column
|
|
m21 *= alpha;
|
|
m31 *= alpha;
|
|
m12 *= beta; // Renormalize second column
|
|
m22 *= beta;
|
|
m32 *= beta;
|
|
m13 *= gamma;
|
|
m23 *= gamma;
|
|
m33 *= gamma;
|
|
alpha = m11 * m12 + m21 * m22 + m31 * m32; // First and second column dot product
|
|
beta = m11 * m13 + m21 * m23 + m31 * m33; // First and third column dot product
|
|
gamma = m12 * m13 + m22 * m23 + m32 * m33; // Second and third column dot product
|
|
alpha *= 0.5;
|
|
beta *= 0.5;
|
|
gamma *= 0.5;
|
|
double temp1, temp2;
|
|
temp1 = m11 - alpha * m12 - beta * m13; // Update row1
|
|
temp2 = m12 - alpha * m11 - gamma * m13;
|
|
m13 -= beta * m11 + gamma * m12;
|
|
m11 = temp1;
|
|
m12 = temp2;
|
|
temp1 = m21 - alpha * m22 - beta * m23; // Update row2
|
|
temp2 = m22 - alpha * m21 - gamma * m23;
|
|
m23 -= beta * m21 + gamma * m22;
|
|
m21 = temp1;
|
|
m22 = temp2;
|
|
temp1 = m31 - alpha * m32 - beta * m33; // Update row3
|
|
temp2 = m32 - alpha * m31 - gamma * m33;
|
|
m33 -= beta * m31 + gamma * m32;
|
|
m31 = temp1;
|
|
m32 = temp2;
|
|
return *this;
|
|
}
|
|
|
|
void Matrix3x3::OperatorTimesEquals(const Matrix3x3& B) // Matrix product
|
|
{
|
|
double t1, t2; // temporary values
|
|
t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31;
|
|
t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32;
|
|
m13 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33;
|
|
m11 = t1;
|
|
m12 = t2;
|
|
|
|
t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31;
|
|
t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32;
|
|
m23 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33;
|
|
m21 = t1;
|
|
m22 = t2;
|
|
|
|
t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31;
|
|
t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32;
|
|
m33 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33;
|
|
m31 = t1;
|
|
m32 = t2;
|
|
return;
|
|
}
|
|
|
|
VectorR3 Matrix3x3::Solve(const VectorR3& u) const // Returns solution
|
|
{ // based on Cramer's rule
|
|
double sd11 = m22 * m33 - m23 * m32;
|
|
double sd21 = m32 * m13 - m12 * m33;
|
|
double sd31 = m12 * m23 - m22 * m13;
|
|
double sd12 = m31 * m23 - m21 * m33;
|
|
double sd22 = m11 * m33 - m31 * m13;
|
|
double sd32 = m21 * m13 - m11 * m23;
|
|
double sd13 = m21 * m32 - m31 * m22;
|
|
double sd23 = m31 * m12 - m11 * m32;
|
|
double sd33 = m11 * m22 - m21 * m12;
|
|
|
|
double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
|
|
|
|
double rx = (u.x * sd11 + u.y * sd21 + u.z * sd31) * detInv;
|
|
double ry = (u.x * sd12 + u.y * sd22 + u.z * sd32) * detInv;
|
|
double rz = (u.x * sd13 + u.y * sd23 + u.z * sd33) * detInv;
|
|
|
|
return (VectorR3(rx, ry, rz));
|
|
}
|
|
|
|
// ******************************************************
|
|
// * Matrix3x4 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
Matrix3x4& Matrix3x4::ReNormalize() // Re-normalizes nearly orthonormal matrix
|
|
{
|
|
double alpha = m11 * m11 + m21 * m21 + m31 * m31; // First column's norm squared
|
|
double beta = m12 * m12 + m22 * m22 + m32 * m32; // Second column's norm squared
|
|
double gamma = m13 * m13 + m23 * m23 + m33 * m33; // Third column's norm squared
|
|
alpha = 1.0 - 0.5 * (alpha - 1.0); // Get mult. factor
|
|
beta = 1.0 - 0.5 * (beta - 1.0);
|
|
gamma = 1.0 - 0.5 * (gamma - 1.0);
|
|
m11 *= alpha; // Renormalize first column
|
|
m21 *= alpha;
|
|
m31 *= alpha;
|
|
m12 *= beta; // Renormalize second column
|
|
m22 *= beta;
|
|
m32 *= beta;
|
|
m13 *= gamma;
|
|
m23 *= gamma;
|
|
m33 *= gamma;
|
|
alpha = m11 * m12 + m21 * m22 + m31 * m32; // First and second column dot product
|
|
beta = m11 * m13 + m21 * m23 + m31 * m33; // First and third column dot product
|
|
gamma = m12 * m13 + m22 * m23 + m32 * m33; // Second and third column dot product
|
|
alpha *= 0.5;
|
|
beta *= 0.5;
|
|
gamma *= 0.5;
|
|
double temp1, temp2;
|
|
temp1 = m11 - alpha * m12 - beta * m13; // Update row1
|
|
temp2 = m12 - alpha * m11 - gamma * m13;
|
|
m13 -= beta * m11 + gamma * m12;
|
|
m11 = temp1;
|
|
m12 = temp2;
|
|
temp1 = m21 - alpha * m22 - beta * m23; // Update row2
|
|
temp2 = m22 - alpha * m21 - gamma * m23;
|
|
m23 -= beta * m21 + gamma * m22;
|
|
m21 = temp1;
|
|
m22 = temp2;
|
|
temp1 = m31 - alpha * m32 - beta * m33; // Update row3
|
|
temp2 = m32 - alpha * m31 - gamma * m33;
|
|
m33 -= beta * m31 + gamma * m32;
|
|
m31 = temp1;
|
|
m32 = temp2;
|
|
return *this;
|
|
}
|
|
|
|
void Matrix3x4::OperatorTimesEquals(const Matrix3x4& B) // Composition
|
|
{
|
|
m14 += m11 * B.m14 + m12 * B.m24 + m13 * B.m34;
|
|
m24 += m21 * B.m14 + m22 * B.m24 + m23 * B.m34;
|
|
m34 += m31 * B.m14 + m32 * B.m24 + m33 * B.m34;
|
|
|
|
double t1, t2; // temporary values
|
|
t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31;
|
|
t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32;
|
|
m13 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33;
|
|
m11 = t1;
|
|
m12 = t2;
|
|
|
|
t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31;
|
|
t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32;
|
|
m23 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33;
|
|
m21 = t1;
|
|
m22 = t2;
|
|
|
|
t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31;
|
|
t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32;
|
|
m33 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33;
|
|
m31 = t1;
|
|
m32 = t2;
|
|
}
|
|
|
|
void Matrix3x4::OperatorTimesEquals(const Matrix3x3& B) // Composition
|
|
{
|
|
double t1, t2; // temporary values
|
|
t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31;
|
|
t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32;
|
|
m13 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33;
|
|
m11 = t1;
|
|
m12 = t2;
|
|
|
|
t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31;
|
|
t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32;
|
|
m23 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33;
|
|
m21 = t1;
|
|
m22 = t2;
|
|
|
|
t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31;
|
|
t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32;
|
|
m33 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33;
|
|
m31 = t1;
|
|
m32 = t2;
|
|
}
|
|
|
|
// ******************************************************
|
|
// * LinearMapR3 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
LinearMapR3 operator*(const LinearMapR3& A, const LinearMapR3& B)
|
|
{
|
|
return (LinearMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
|
|
A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
|
|
A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
|
|
A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
|
|
A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
|
|
A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
|
|
A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
|
|
A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
|
|
A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33));
|
|
}
|
|
|
|
double LinearMapR3::Determinant() const // Returns the determinant
|
|
{
|
|
return (m11 * (m22 * m33 - m23 * m32) - m12 * (m21 * m33 - m31 * m23) + m13 * (m21 * m23 - m31 * m22));
|
|
}
|
|
|
|
LinearMapR3 LinearMapR3::Inverse() const // Returns inverse
|
|
{
|
|
double sd11 = m22 * m33 - m23 * m32;
|
|
double sd21 = m32 * m13 - m12 * m33;
|
|
double sd31 = m12 * m23 - m22 * m13;
|
|
double sd12 = m31 * m23 - m21 * m33;
|
|
double sd22 = m11 * m33 - m31 * m13;
|
|
double sd32 = m21 * m13 - m11 * m23;
|
|
double sd13 = m21 * m32 - m31 * m22;
|
|
double sd23 = m31 * m12 - m11 * m32;
|
|
double sd33 = m11 * m22 - m21 * m12;
|
|
|
|
double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
|
|
|
|
return (LinearMapR3(sd11 * detInv, sd12 * detInv, sd13 * detInv,
|
|
sd21 * detInv, sd22 * detInv, sd23 * detInv,
|
|
sd31 * detInv, sd32 * detInv, sd33 * detInv));
|
|
}
|
|
|
|
LinearMapR3& LinearMapR3::Invert() // Converts into inverse.
|
|
{
|
|
double sd11 = m22 * m33 - m23 * m32;
|
|
double sd21 = m32 * m13 - m12 * m33;
|
|
double sd31 = m12 * m23 - m22 * m13;
|
|
double sd12 = m31 * m23 - m21 * m33;
|
|
double sd22 = m11 * m33 - m31 * m13;
|
|
double sd32 = m21 * m13 - m11 * m23;
|
|
double sd13 = m21 * m32 - m31 * m22;
|
|
double sd23 = m31 * m12 - m11 * m32;
|
|
double sd33 = m11 * m22 - m21 * m12;
|
|
|
|
double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
|
|
|
|
m11 = sd11 * detInv;
|
|
m12 = sd21 * detInv;
|
|
m13 = sd31 * detInv;
|
|
m21 = sd12 * detInv;
|
|
m22 = sd22 * detInv;
|
|
m23 = sd32 * detInv;
|
|
m31 = sd13 * detInv;
|
|
m32 = sd23 * detInv;
|
|
m33 = sd33 * detInv;
|
|
|
|
return (*this);
|
|
}
|
|
|
|
// ******************************************************
|
|
// * AffineMapR3 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
AffineMapR3 operator*(const AffineMapR3& A, const AffineMapR3& B)
|
|
{
|
|
return (AffineMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
|
|
A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
|
|
A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
|
|
A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
|
|
A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
|
|
A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
|
|
A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
|
|
A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
|
|
A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33,
|
|
A.m11 * B.m14 + A.m12 * B.m24 + A.m13 * B.m34 + A.m14,
|
|
A.m21 * B.m14 + A.m22 * B.m24 + A.m23 * B.m34 + A.m24,
|
|
A.m31 * B.m14 + A.m32 * B.m24 + A.m33 * B.m34 + A.m34));
|
|
}
|
|
|
|
AffineMapR3 operator*(const LinearMapR3& A, const AffineMapR3& B)
|
|
{
|
|
return (AffineMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
|
|
A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
|
|
A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
|
|
A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
|
|
A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
|
|
A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
|
|
A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
|
|
A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
|
|
A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33,
|
|
A.m11 * B.m14 + A.m12 * B.m24 + A.m13 * B.m34,
|
|
A.m21 * B.m14 + A.m22 * B.m24 + A.m23 * B.m34,
|
|
A.m31 * B.m14 + A.m32 * B.m24 + A.m33 * B.m34));
|
|
}
|
|
|
|
AffineMapR3 operator*(const AffineMapR3& A, const LinearMapR3& B)
|
|
{
|
|
return (AffineMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
|
|
A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
|
|
A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
|
|
A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
|
|
A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
|
|
A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
|
|
A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
|
|
A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
|
|
A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33,
|
|
A.m14,
|
|
A.m24,
|
|
A.m34));
|
|
}
|
|
|
|
AffineMapR3 AffineMapR3::Inverse() const // Returns inverse
|
|
{
|
|
double sd11 = m22 * m33 - m23 * m32;
|
|
double sd21 = m32 * m13 - m12 * m33;
|
|
double sd31 = m12 * m23 - m22 * m13;
|
|
double sd12 = m31 * m23 - m21 * m33;
|
|
double sd22 = m11 * m33 - m31 * m13;
|
|
double sd32 = m21 * m13 - m11 * m23;
|
|
double sd13 = m21 * m32 - m31 * m22;
|
|
double sd23 = m31 * m12 - m11 * m32;
|
|
double sd33 = m11 * m22 - m21 * m12;
|
|
|
|
double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
|
|
|
|
// Make sd's hold the (transpose of) the inverse of the 3x3 part
|
|
sd11 *= detInv;
|
|
sd12 *= detInv;
|
|
sd13 *= detInv;
|
|
sd21 *= detInv;
|
|
sd22 *= detInv;
|
|
sd23 *= detInv;
|
|
sd31 *= detInv;
|
|
sd32 *= detInv;
|
|
sd33 *= detInv;
|
|
double sd41 = -(m14 * sd11 + m24 * sd21 + m34 * sd31);
|
|
double sd42 = -(m14 * sd12 + m24 * sd22 + m34 * sd32);
|
|
double sd43 = -(m14 * sd12 + m24 * sd23 + m34 * sd33);
|
|
|
|
return (AffineMapR3(sd11, sd12, sd13,
|
|
sd21, sd22, sd23,
|
|
sd31, sd32, sd33,
|
|
sd41, sd42, sd43));
|
|
}
|
|
|
|
AffineMapR3& AffineMapR3::Invert() // Converts into inverse.
|
|
{
|
|
double sd11 = m22 * m33 - m23 * m32;
|
|
double sd21 = m32 * m13 - m12 * m33;
|
|
double sd31 = m12 * m23 - m22 * m13;
|
|
double sd12 = m31 * m23 - m21 * m33;
|
|
double sd22 = m11 * m33 - m31 * m13;
|
|
double sd32 = m21 * m13 - m11 * m23;
|
|
double sd13 = m21 * m32 - m31 * m22;
|
|
double sd23 = m31 * m12 - m11 * m32;
|
|
double sd33 = m11 * m22 - m21 * m12;
|
|
|
|
double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
|
|
|
|
m11 = sd11 * detInv;
|
|
m12 = sd21 * detInv;
|
|
m13 = sd31 * detInv;
|
|
m21 = sd12 * detInv;
|
|
m22 = sd22 * detInv;
|
|
m23 = sd32 * detInv;
|
|
m31 = sd13 * detInv;
|
|
m32 = sd23 * detInv;
|
|
m33 = sd33 * detInv;
|
|
double sd41 = -(m14 * m11 + m24 * m12 + m34 * m13); // Compare to ::Inverse.
|
|
double sd42 = -(m14 * m21 + m24 * m22 + m34 * m23);
|
|
double sd43 = -(m14 * m31 + m24 * m32 + m34 * m33);
|
|
m14 = sd41;
|
|
m24 = sd42;
|
|
m34 = sd43;
|
|
|
|
return (*this);
|
|
}
|
|
|
|
// **************************************************************
|
|
// * RotationMapR3 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
RotationMapR3 operator*(const RotationMapR3& A, const RotationMapR3& B)
|
|
// Matrix product (composition)
|
|
{
|
|
return (RotationMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
|
|
A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
|
|
A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
|
|
A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
|
|
A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
|
|
A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
|
|
A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
|
|
A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
|
|
A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33));
|
|
}
|
|
|
|
RotationMapR3& RotationMapR3::Set(const Quaternion& quat)
|
|
{
|
|
double wSq = quat.w * quat.w;
|
|
double xSq = quat.x * quat.x;
|
|
double ySq = quat.y * quat.y;
|
|
double zSq = quat.z * quat.z;
|
|
double Dqwx = 2.0 * quat.w * quat.x;
|
|
double Dqwy = 2.0 * quat.w * quat.y;
|
|
double Dqwz = 2.0 * quat.w * quat.z;
|
|
double Dqxy = 2.0 * quat.x * quat.y;
|
|
double Dqyz = 2.0 * quat.y * quat.z;
|
|
double Dqxz = 2.0 * quat.x * quat.z;
|
|
m11 = wSq + xSq - ySq - zSq;
|
|
m22 = wSq - xSq + ySq - zSq;
|
|
m33 = wSq - xSq - ySq + zSq;
|
|
m12 = Dqxy - Dqwz;
|
|
m21 = Dqxy + Dqwz;
|
|
m13 = Dqxz + Dqwy;
|
|
m31 = Dqxz - Dqwy;
|
|
m23 = Dqyz - Dqwx;
|
|
m32 = Dqyz + Dqwx;
|
|
return *this;
|
|
}
|
|
|
|
RotationMapR3& RotationMapR3::Set(const VectorR3& u, double theta)
|
|
{
|
|
assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
|
|
double c = cos(theta);
|
|
double s = sin(theta);
|
|
double mc = 1.0 - c;
|
|
double xmc = u.x * mc;
|
|
double xymc = xmc * u.y;
|
|
double xzmc = xmc * u.z;
|
|
double yzmc = u.y * u.z * mc;
|
|
double xs = u.x * s;
|
|
double ys = u.y * s;
|
|
double zs = u.z * s;
|
|
Matrix3x3::Set(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
|
|
xymc - zs, u.y * u.y * mc + c, yzmc + xs,
|
|
xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
|
|
return *this;
|
|
}
|
|
|
|
// The rotation axis vector u MUST be a UNIT vector!!!
|
|
RotationMapR3& RotationMapR3::Set(const VectorR3& u, double s, double c)
|
|
{
|
|
assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
|
|
double mc = 1.0 - c;
|
|
double xmc = u.x * mc;
|
|
double xymc = xmc * u.y;
|
|
double xzmc = xmc * u.z;
|
|
double yzmc = u.y * u.z * mc;
|
|
double xs = u.x * s;
|
|
double ys = u.y * s;
|
|
double zs = u.z * s;
|
|
Matrix3x3::Set(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
|
|
xymc - zs, u.y * u.y * mc + c, yzmc + xs,
|
|
xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
|
|
return *this;
|
|
}
|
|
|
|
// ToAxisAndAngle - find a unit vector in the direction of the rotation axis,
|
|
// and the angle theta of rotation. Returns true if the rotation angle is non-zero,
|
|
// and false if it is zero.
|
|
bool RotationMapR3::ToAxisAndAngle(VectorR3* u, double* theta) const
|
|
{
|
|
double alpha = m11 + m22 + m33 - 1.0;
|
|
double beta = sqrt(Square(m32 - m23) + Square(m13 - m31) + Square(m21 - m12));
|
|
if (beta == 0.0)
|
|
{
|
|
*u = VectorR3::UnitY;
|
|
*theta = 0.0;
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
u->Set(m32 - m23, m13 - m31, m21 - m12);
|
|
*u /= beta;
|
|
*theta = atan2(beta, alpha);
|
|
return true;
|
|
}
|
|
}
|
|
|
|
// VrRotate is similar to glRotate. Returns a matrix (LinearMapR3)
|
|
// that will perform the rotation when multiplied on the left.
|
|
// u is supposed to be a *unit* vector. Otherwise, the LinearMapR3
|
|
// returned will be garbage!
|
|
RotationMapR3 VrRotate(double theta, const VectorR3& u)
|
|
{
|
|
RotationMapR3 ret;
|
|
ret.Set(u, theta);
|
|
return ret;
|
|
}
|
|
|
|
// This version of rotate takes the cosine and sine of theta
|
|
// instead of theta. u must still be a unit vector.
|
|
|
|
RotationMapR3 VrRotate(double c, double s, const VectorR3& u)
|
|
{
|
|
RotationMapR3 ret;
|
|
ret.Set(u, s, c);
|
|
return ret;
|
|
}
|
|
|
|
// Returns a RotationMapR3 which rotates the fromVec to be colinear
|
|
// with toVec.
|
|
|
|
RotationMapR3 VrRotateAlign(const VectorR3& fromVec, const VectorR3& toVec)
|
|
{
|
|
VectorR3 crossVec = fromVec;
|
|
crossVec *= toVec;
|
|
double sinetheta = crossVec.Norm(); // Not yet normalized!
|
|
if (sinetheta < 1.0e-40)
|
|
{
|
|
return (RotationMapR3(
|
|
1.0, 0.0, 0.0,
|
|
0.0, 1.0, 0.0,
|
|
0.0, 0.0, 1.0));
|
|
}
|
|
crossVec /= sinetheta; // Normalize the vector
|
|
double scale = 1.0 / sqrt(fromVec.NormSq() * toVec.NormSq());
|
|
sinetheta *= scale;
|
|
double cosinetheta = (fromVec ^ toVec) * scale;
|
|
return (VrRotate(cosinetheta, sinetheta, crossVec));
|
|
}
|
|
|
|
// RotateToMap returns a rotation map which rotates fromVec to have the
|
|
// same direction as toVec.
|
|
// fromVec and toVec should be unit vectors
|
|
RotationMapR3 RotateToMap(const VectorR3& fromVec, const VectorR3& toVec)
|
|
{
|
|
VectorR3 crossVec = fromVec;
|
|
crossVec *= toVec;
|
|
double sintheta = crossVec.Norm();
|
|
double costheta = fromVec ^ toVec;
|
|
if (sintheta <= 1.0e-40)
|
|
{
|
|
if (costheta > 0.0)
|
|
{
|
|
return (RotationMapR3(
|
|
1.0, 0.0, 0.0,
|
|
0.0, 1.0, 0.0,
|
|
0.0, 0.0, 1.0));
|
|
}
|
|
else
|
|
{
|
|
GetOrtho(toVec, crossVec); // Get arbitrary orthonormal vector
|
|
return (VrRotate(costheta, sintheta, crossVec));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
crossVec /= sintheta; // Normalize the vector
|
|
return (VrRotate(costheta, sintheta, crossVec));
|
|
}
|
|
}
|
|
|
|
// **************************************************************
|
|
// * RigidMapR3 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
// The rotation axis vector u MUST be a UNIT vector!!!
|
|
RigidMapR3& RigidMapR3::SetRotationPart(const VectorR3& u, double theta)
|
|
{
|
|
assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
|
|
double c = cos(theta);
|
|
double s = sin(theta);
|
|
double mc = 1.0 - c;
|
|
double xmc = u.x * mc;
|
|
double xymc = xmc * u.y;
|
|
double xzmc = xmc * u.z;
|
|
double yzmc = u.y * u.z * mc;
|
|
double xs = u.x * s;
|
|
double ys = u.y * s;
|
|
double zs = u.z * s;
|
|
Matrix3x4::Set3x3(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
|
|
xymc - zs, u.y * u.y * mc + c, yzmc + xs,
|
|
xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
|
|
return *this;
|
|
}
|
|
|
|
// The rotation axis vector u MUST be a UNIT vector!!!
|
|
RigidMapR3& RigidMapR3::SetRotationPart(const VectorR3& u, double s, double c)
|
|
{
|
|
assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
|
|
double mc = 1.0 - c;
|
|
double xmc = u.x * mc;
|
|
double xymc = xmc * u.y;
|
|
double xzmc = xmc * u.z;
|
|
double yzmc = u.y * u.z * mc;
|
|
double xs = u.x * s;
|
|
double ys = u.y * s;
|
|
double zs = u.z * s;
|
|
Matrix3x4::Set3x3(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
|
|
xymc - zs, u.y * u.y * mc + c, yzmc + xs,
|
|
xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
|
|
return *this;
|
|
}
|
|
|
|
// CalcGlideRotation - converts a rigid map into an equivalent
|
|
// glide rotation (screw motion). It returns the rotation axis
|
|
// as base point u, and a rotation axis v. The vectors u and v are
|
|
// always orthonormal. v will be a unit vector.
|
|
// It also returns the glide distance, which is the translation parallel
|
|
// to v. Further, it returns the signed rotation angle theta (right hand rule
|
|
// specifies the direction.
|
|
// The glide rotation means a rotation around the point u with axis direction v.
|
|
// Return code "true" if the rotation amount is non-zero. "false" if pure translation.
|
|
bool RigidMapR3::CalcGlideRotation(VectorR3* u, VectorR3* v,
|
|
double* glideDist, double* rotation) const
|
|
{
|
|
// Compare to the code for ToAxisAndAngle.
|
|
double alpha = m11 + m22 + m33 - 1.0;
|
|
double beta = sqrt(Square(m32 - m23) + Square(m13 - m31) + Square(m21 - m12));
|
|
if (beta == 0.0)
|
|
{
|
|
double vN = m14 * m14 + m24 * m24 + m34 * m34;
|
|
if (vN > 0.0)
|
|
{
|
|
vN = sqrt(vN);
|
|
v->Set(m14, m24, m34);
|
|
*v /= vN;
|
|
*glideDist = vN;
|
|
}
|
|
else
|
|
{
|
|
*v = VectorR3::UnitX;
|
|
*glideDist = 0.0;
|
|
}
|
|
u->SetZero();
|
|
*rotation = 0;
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
v->Set(m32 - m23, m13 - m31, m21 - m12);
|
|
*v /= beta; // v - unit vector, rotation axis
|
|
*rotation = atan2(beta, alpha);
|
|
u->Set(m14, m24, m34);
|
|
*glideDist = ((*u) ^ (*v));
|
|
VectorR3 temp = *v;
|
|
temp *= *glideDist;
|
|
*u -= temp; // Subtract component in direction of rot. axis v
|
|
temp = *v;
|
|
temp *= *u;
|
|
temp /= tan((*rotation) / 2); // temp = (v \times u) / tan(rotation/2)
|
|
*u += temp;
|
|
*u *= 0.5;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
// ***************************************************************
|
|
// Linear Algebra Utilities *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
// Returns a righthanded orthonormal basis to complement vector u
|
|
void GetOrtho(const VectorR3& u, VectorR3& v, VectorR3& w)
|
|
{
|
|
if (u.x > 0.5 || u.x < -0.5 || u.y > 0.5 || u.y < -0.5)
|
|
{
|
|
v.Set(u.y, -u.x, 0.0);
|
|
}
|
|
else
|
|
{
|
|
v.Set(0.0, u.z, -u.y);
|
|
}
|
|
v.Normalize();
|
|
w = u;
|
|
w *= v;
|
|
w.Normalize();
|
|
// w.NormalizeFast();
|
|
return;
|
|
}
|
|
|
|
// Returns a vector v orthonormal to unit vector u
|
|
void GetOrtho(const VectorR3& u, VectorR3& v)
|
|
{
|
|
if (u.x > 0.5 || u.x < -0.5 || u.y > 0.5 || u.y < -0.5)
|
|
{
|
|
v.Set(u.y, -u.x, 0.0);
|
|
}
|
|
else
|
|
{
|
|
v.Set(0.0, u.z, -u.y);
|
|
}
|
|
v.Normalize();
|
|
return;
|
|
}
|
|
|
|
// ***************************************************************
|
|
// Stream Output Routines *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
ostream& operator<<(ostream& os, const VectorR3& u)
|
|
{
|
|
return (os << "<" << u.x << "," << u.y << "," << u.z << ">");
|
|
}
|
|
|
|
ostream& operator<<(ostream& os, const Matrix3x3& A)
|
|
{
|
|
os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13 << ">\n"
|
|
<< " <" << A.m21 << ", " << A.m22 << ", " << A.m23 << ">\n"
|
|
<< " <" << A.m31 << ", " << A.m32 << ", " << A.m33 << ">\n";
|
|
return (os);
|
|
}
|
|
|
|
ostream& operator<<(ostream& os, const Matrix3x4& A)
|
|
{
|
|
os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13
|
|
<< "; " << A.m14 << ">\n"
|
|
<< " <" << A.m21 << ", " << A.m22 << ", " << A.m23
|
|
<< "; " << A.m24 << ">\n"
|
|
<< " <" << A.m31 << ", " << A.m32 << ", " << A.m33
|
|
<< "; " << A.m34 << ">\n";
|
|
return (os);
|
|
}
|