mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +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
468 lines
17 KiB
C++
468 lines
17 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 "LinearR4.h"
|
|
|
|
#include <assert.h>
|
|
|
|
const VectorR4 VectorR4::Zero(0.0, 0.0, 0.0, 0.0);
|
|
const VectorR4 VectorR4::UnitX(1.0, 0.0, 0.0, 0.0);
|
|
const VectorR4 VectorR4::UnitY(0.0, 1.0, 0.0, 0.0);
|
|
const VectorR4 VectorR4::UnitZ(0.0, 0.0, 1.0, 0.0);
|
|
const VectorR4 VectorR4::UnitW(0.0, 0.0, 0.0, 1.0);
|
|
const VectorR4 VectorR4::NegUnitX(-1.0, 0.0, 0.0, 0.0);
|
|
const VectorR4 VectorR4::NegUnitY(0.0, -1.0, 0.0, 0.0);
|
|
const VectorR4 VectorR4::NegUnitZ(0.0, 0.0, -1.0, 0.0);
|
|
const VectorR4 VectorR4::NegUnitW(0.0, 0.0, 0.0, -1.0);
|
|
|
|
const Matrix4x4 Matrix4x4::Identity(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
|
|
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
|
|
|
|
// ******************************************************
|
|
// * VectorR4 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
double VectorR4::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;
|
|
if (w > m)
|
|
m = w;
|
|
else if (-w > m)
|
|
m = -w;
|
|
return m;
|
|
}
|
|
|
|
// ******************************************************
|
|
// * Matrix4x4 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
void Matrix4x4::operator*=(const Matrix4x4& B) // Matrix product
|
|
{
|
|
double t1, t2, t3; // temporary values
|
|
t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31 + m14 * B.m41;
|
|
t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32 + m14 * B.m42;
|
|
t3 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33 + m14 * B.m43;
|
|
m14 = m11 * B.m14 + m12 * B.m24 + m13 * B.m34 + m14 * B.m44;
|
|
m11 = t1;
|
|
m12 = t2;
|
|
m13 = t3;
|
|
|
|
t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31 + m24 * B.m41;
|
|
t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32 + m24 * B.m42;
|
|
t3 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33 + m24 * B.m43;
|
|
m24 = m21 * B.m14 + m22 * B.m24 + m23 * B.m34 + m24 * B.m44;
|
|
m21 = t1;
|
|
m22 = t2;
|
|
m23 = t3;
|
|
|
|
t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31 + m34 * B.m41;
|
|
t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32 + m34 * B.m42;
|
|
t3 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33 + m34 * B.m43;
|
|
m34 = m31 * B.m14 + m32 * B.m24 + m33 * B.m34 + m34 * B.m44;
|
|
m31 = t1;
|
|
m32 = t2;
|
|
m33 = t3;
|
|
|
|
t1 = m41 * B.m11 + m42 * B.m21 + m43 * B.m31 + m44 * B.m41;
|
|
t2 = m41 * B.m12 + m42 * B.m22 + m43 * B.m32 + m44 * B.m42;
|
|
t3 = m41 * B.m13 + m42 * B.m23 + m43 * B.m33 + m44 * B.m43;
|
|
m44 = m41 * B.m14 + m42 * B.m24 + m43 * B.m34 + m44 * B.m44;
|
|
m41 = t1;
|
|
m42 = t2;
|
|
m43 = t3;
|
|
}
|
|
|
|
inline void ReNormalizeHelper(double& a, double& b, double& c, double& d)
|
|
{
|
|
double scaleF = a * a + b * b + c * c + d * d; // Inner product of Vector-R4
|
|
scaleF = 1.0 - 0.5 * (scaleF - 1.0);
|
|
a *= scaleF;
|
|
b *= scaleF;
|
|
c *= scaleF;
|
|
d *= scaleF;
|
|
}
|
|
|
|
Matrix4x4& Matrix4x4::ReNormalize()
|
|
{
|
|
ReNormalizeHelper(m11, m21, m31, m41); // Renormalize first column
|
|
ReNormalizeHelper(m12, m22, m32, m42); // Renormalize second column
|
|
ReNormalizeHelper(m13, m23, m33, m43); // Renormalize third column
|
|
ReNormalizeHelper(m14, m24, m34, m44); // Renormalize fourth column
|
|
double alpha = 0.5 * (m11 * m12 + m21 * m22 + m31 * m32 + m41 * m42); //1st and 2nd cols
|
|
double beta = 0.5 * (m11 * m13 + m21 * m23 + m31 * m33 + m41 * m43); //1st and 3rd cols
|
|
double gamma = 0.5 * (m11 * m14 + m21 * m24 + m31 * m34 + m41 * m44); //1st and 4nd cols
|
|
double delta = 0.5 * (m12 * m13 + m22 * m23 + m32 * m33 + m42 * m43); //2nd and 3rd cols
|
|
double eps = 0.5 * (m12 * m14 + m22 * m24 + m32 * m34 + m42 * m44); //2nd and 4nd cols
|
|
double phi = 0.5 * (m13 * m14 + m23 * m24 + m33 * m34 + m43 * m44); //3rd and 4nd cols
|
|
double temp1, temp2, temp3;
|
|
temp1 = m11 - alpha * m12 - beta * m13 - gamma * m14;
|
|
temp2 = m12 - alpha * m11 - delta * m13 - eps * m14;
|
|
temp3 = m13 - beta * m11 - delta * m12 - phi * m14;
|
|
m14 -= (gamma * m11 + eps * m12 + phi * m13);
|
|
m11 = temp1;
|
|
m12 = temp2;
|
|
m13 = temp3;
|
|
temp1 = m21 - alpha * m22 - beta * m23 - gamma * m24;
|
|
temp2 = m22 - alpha * m21 - delta * m23 - eps * m24;
|
|
temp3 = m23 - beta * m21 - delta * m22 - phi * m24;
|
|
m24 -= (gamma * m21 + eps * m22 + phi * m23);
|
|
m21 = temp1;
|
|
m22 = temp2;
|
|
m23 = temp3;
|
|
temp1 = m31 - alpha * m32 - beta * m33 - gamma * m34;
|
|
temp2 = m32 - alpha * m31 - delta * m33 - eps * m34;
|
|
temp3 = m33 - beta * m31 - delta * m32 - phi * m34;
|
|
m34 -= (gamma * m31 + eps * m32 + phi * m33);
|
|
m31 = temp1;
|
|
m32 = temp2;
|
|
m33 = temp3;
|
|
temp1 = m41 - alpha * m42 - beta * m43 - gamma * m44;
|
|
temp2 = m42 - alpha * m41 - delta * m43 - eps * m44;
|
|
temp3 = m43 - beta * m41 - delta * m42 - phi * m44;
|
|
m44 -= (gamma * m41 + eps * m42 + phi * m43);
|
|
m41 = temp1;
|
|
m42 = temp2;
|
|
m43 = temp3;
|
|
return *this;
|
|
}
|
|
|
|
// ******************************************************
|
|
// * LinearMapR4 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
double LinearMapR4::Determinant() const // Returns the determinant
|
|
{
|
|
double Tbt34C12 = m31 * m42 - m32 * m41; // 2x2 subdeterminants
|
|
double Tbt34C13 = m31 * m43 - m33 * m41;
|
|
double Tbt34C14 = m31 * m44 - m34 * m41;
|
|
double Tbt34C23 = m32 * m43 - m33 * m42;
|
|
double Tbt34C24 = m32 * m44 - m34 * m42;
|
|
double Tbt34C34 = m33 * m44 - m34 * m43;
|
|
|
|
double sd11 = m22 * Tbt34C34 - m23 * Tbt34C24 + m24 * Tbt34C23; // 3x3 subdeterminants
|
|
double sd12 = m21 * Tbt34C34 - m23 * Tbt34C14 + m24 * Tbt34C13;
|
|
double sd13 = m21 * Tbt34C24 - m22 * Tbt34C14 + m24 * Tbt34C12;
|
|
double sd14 = m21 * Tbt34C23 - m22 * Tbt34C13 + m23 * Tbt34C12;
|
|
|
|
return (m11 * sd11 - m12 * sd12 + m13 * sd13 - m14 * sd14);
|
|
}
|
|
|
|
LinearMapR4 LinearMapR4::Inverse() const // Returns inverse
|
|
{
|
|
double Tbt34C12 = m31 * m42 - m32 * m41; // 2x2 subdeterminants
|
|
double Tbt34C13 = m31 * m43 - m33 * m41;
|
|
double Tbt34C14 = m31 * m44 - m34 * m41;
|
|
double Tbt34C23 = m32 * m43 - m33 * m42;
|
|
double Tbt34C24 = m32 * m44 - m34 * m42;
|
|
double Tbt34C34 = m33 * m44 - m34 * m43;
|
|
double Tbt24C12 = m21 * m42 - m22 * m41; // 2x2 subdeterminants
|
|
double Tbt24C13 = m21 * m43 - m23 * m41;
|
|
double Tbt24C14 = m21 * m44 - m24 * m41;
|
|
double Tbt24C23 = m22 * m43 - m23 * m42;
|
|
double Tbt24C24 = m22 * m44 - m24 * m42;
|
|
double Tbt24C34 = m23 * m44 - m24 * m43;
|
|
double Tbt23C12 = m21 * m32 - m22 * m31; // 2x2 subdeterminants
|
|
double Tbt23C13 = m21 * m33 - m23 * m31;
|
|
double Tbt23C14 = m21 * m34 - m24 * m31;
|
|
double Tbt23C23 = m22 * m33 - m23 * m32;
|
|
double Tbt23C24 = m22 * m34 - m24 * m32;
|
|
double Tbt23C34 = m23 * m34 - m24 * m33;
|
|
|
|
double sd11 = m22 * Tbt34C34 - m23 * Tbt34C24 + m24 * Tbt34C23; // 3x3 subdeterminants
|
|
double sd12 = m21 * Tbt34C34 - m23 * Tbt34C14 + m24 * Tbt34C13;
|
|
double sd13 = m21 * Tbt34C24 - m22 * Tbt34C14 + m24 * Tbt34C12;
|
|
double sd14 = m21 * Tbt34C23 - m22 * Tbt34C13 + m23 * Tbt34C12;
|
|
double sd21 = m12 * Tbt34C34 - m13 * Tbt34C24 + m14 * Tbt34C23; // 3x3 subdeterminants
|
|
double sd22 = m11 * Tbt34C34 - m13 * Tbt34C14 + m14 * Tbt34C13;
|
|
double sd23 = m11 * Tbt34C24 - m12 * Tbt34C14 + m14 * Tbt34C12;
|
|
double sd24 = m11 * Tbt34C23 - m12 * Tbt34C13 + m13 * Tbt34C12;
|
|
double sd31 = m12 * Tbt24C34 - m13 * Tbt24C24 + m14 * Tbt24C23; // 3x3 subdeterminants
|
|
double sd32 = m11 * Tbt24C34 - m13 * Tbt24C14 + m14 * Tbt24C13;
|
|
double sd33 = m11 * Tbt24C24 - m12 * Tbt24C14 + m14 * Tbt24C12;
|
|
double sd34 = m11 * Tbt24C23 - m12 * Tbt24C13 + m13 * Tbt24C12;
|
|
double sd41 = m12 * Tbt23C34 - m13 * Tbt23C24 + m14 * Tbt23C23; // 3x3 subdeterminants
|
|
double sd42 = m11 * Tbt23C34 - m13 * Tbt23C14 + m14 * Tbt23C13;
|
|
double sd43 = m11 * Tbt23C24 - m12 * Tbt23C14 + m14 * Tbt23C12;
|
|
double sd44 = m11 * Tbt23C23 - m12 * Tbt23C13 + m13 * Tbt23C12;
|
|
|
|
double detInv = 1.0 / (m11 * sd11 - m12 * sd12 + m13 * sd13 - m14 * sd14);
|
|
|
|
return (LinearMapR4(sd11 * detInv, -sd12 * detInv, sd13 * detInv, -sd14 * detInv,
|
|
-sd21 * detInv, sd22 * detInv, -sd23 * detInv, sd24 * detInv,
|
|
sd31 * detInv, -sd32 * detInv, sd33 * detInv, -sd34 * detInv,
|
|
-sd41 * detInv, sd42 * detInv, -sd43 * detInv, sd44 * detInv));
|
|
}
|
|
|
|
LinearMapR4& LinearMapR4::Invert() // Converts into inverse.
|
|
{
|
|
double Tbt34C12 = m31 * m42 - m32 * m41; // 2x2 subdeterminants
|
|
double Tbt34C13 = m31 * m43 - m33 * m41;
|
|
double Tbt34C14 = m31 * m44 - m34 * m41;
|
|
double Tbt34C23 = m32 * m43 - m33 * m42;
|
|
double Tbt34C24 = m32 * m44 - m34 * m42;
|
|
double Tbt34C34 = m33 * m44 - m34 * m43;
|
|
double Tbt24C12 = m21 * m42 - m22 * m41; // 2x2 subdeterminants
|
|
double Tbt24C13 = m21 * m43 - m23 * m41;
|
|
double Tbt24C14 = m21 * m44 - m24 * m41;
|
|
double Tbt24C23 = m22 * m43 - m23 * m42;
|
|
double Tbt24C24 = m22 * m44 - m24 * m42;
|
|
double Tbt24C34 = m23 * m44 - m24 * m43;
|
|
double Tbt23C12 = m21 * m32 - m22 * m31; // 2x2 subdeterminants
|
|
double Tbt23C13 = m21 * m33 - m23 * m31;
|
|
double Tbt23C14 = m21 * m34 - m24 * m31;
|
|
double Tbt23C23 = m22 * m33 - m23 * m32;
|
|
double Tbt23C24 = m22 * m34 - m24 * m32;
|
|
double Tbt23C34 = m23 * m34 - m24 * m33;
|
|
|
|
double sd11 = m22 * Tbt34C34 - m23 * Tbt34C24 + m24 * Tbt34C23; // 3x3 subdeterminants
|
|
double sd12 = m21 * Tbt34C34 - m23 * Tbt34C14 + m24 * Tbt34C13;
|
|
double sd13 = m21 * Tbt34C24 - m22 * Tbt34C14 + m24 * Tbt34C12;
|
|
double sd14 = m21 * Tbt34C23 - m22 * Tbt34C13 + m23 * Tbt34C12;
|
|
double sd21 = m12 * Tbt34C34 - m13 * Tbt34C24 + m14 * Tbt34C23; // 3x3 subdeterminants
|
|
double sd22 = m11 * Tbt34C34 - m13 * Tbt34C14 + m14 * Tbt34C13;
|
|
double sd23 = m11 * Tbt34C24 - m12 * Tbt34C14 + m14 * Tbt34C12;
|
|
double sd24 = m11 * Tbt34C23 - m12 * Tbt34C13 + m13 * Tbt34C12;
|
|
double sd31 = m12 * Tbt24C34 - m13 * Tbt24C24 + m14 * Tbt24C23; // 3x3 subdeterminants
|
|
double sd32 = m11 * Tbt24C34 - m13 * Tbt24C14 + m14 * Tbt24C13;
|
|
double sd33 = m11 * Tbt24C24 - m12 * Tbt24C14 + m14 * Tbt24C12;
|
|
double sd34 = m11 * Tbt24C23 - m12 * Tbt24C13 + m13 * Tbt24C12;
|
|
double sd41 = m12 * Tbt23C34 - m13 * Tbt23C24 + m14 * Tbt23C23; // 3x3 subdeterminants
|
|
double sd42 = m11 * Tbt23C34 - m13 * Tbt23C14 + m14 * Tbt23C13;
|
|
double sd43 = m11 * Tbt23C24 - m12 * Tbt23C14 + m14 * Tbt23C12;
|
|
double sd44 = m11 * Tbt23C23 - m12 * Tbt23C13 + m13 * Tbt23C12;
|
|
|
|
double detInv = 1.0 / (m11 * sd11 - m12 * sd12 + m13 * sd13 - m14 * sd14);
|
|
|
|
m11 = sd11 * detInv;
|
|
m12 = -sd21 * detInv;
|
|
m13 = sd31 * detInv;
|
|
m14 = -sd41 * detInv;
|
|
m21 = -sd12 * detInv;
|
|
m22 = sd22 * detInv;
|
|
m23 = -sd32 * detInv;
|
|
m24 = sd42 * detInv;
|
|
m31 = sd13 * detInv;
|
|
m32 = -sd23 * detInv;
|
|
m33 = sd33 * detInv;
|
|
m34 = -sd43 * detInv;
|
|
m41 = -sd14 * detInv;
|
|
m42 = sd24 * detInv;
|
|
m43 = -sd34 * detInv;
|
|
m44 = sd44 * detInv;
|
|
|
|
return (*this);
|
|
}
|
|
|
|
VectorR4 LinearMapR4::Solve(const VectorR4& u) const // Returns solution
|
|
{
|
|
// Just uses Inverse() for now.
|
|
return (Inverse() * u);
|
|
}
|
|
|
|
// ******************************************************
|
|
// * RotationMapR4 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
// ***************************************************************
|
|
// * 4-space vector and matrix utilities *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
// Returns u * v^T
|
|
LinearMapR4 TimesTranspose(const VectorR4& u, const VectorR4& v)
|
|
{
|
|
LinearMapR4 result;
|
|
TimesTranspose(u, v, result);
|
|
return result;
|
|
}
|
|
|
|
// The following routines are use to obtain
|
|
// a righthanded orthonormal basis to complement vectors u,v,w.
|
|
// The vectors u,v,w must be unit and orthonormal.
|
|
// The value is returned in "rotmat" with the first column(s) of
|
|
// rotmat equal to u,v,w as appropriate.
|
|
|
|
void GetOrtho(const VectorR4& u, RotationMapR4& rotmat)
|
|
{
|
|
rotmat.SetColumn1(u);
|
|
GetOrtho(1, rotmat);
|
|
}
|
|
|
|
void GetOrtho(const VectorR4& u, const VectorR4& v, RotationMapR4& rotmat)
|
|
{
|
|
rotmat.SetColumn1(u);
|
|
rotmat.SetColumn2(v);
|
|
GetOrtho(2, rotmat);
|
|
}
|
|
|
|
void GetOrtho(const VectorR4& u, const VectorR4& v, const VectorR4& s,
|
|
RotationMapR4& rotmat)
|
|
{
|
|
rotmat.SetColumn1(u);
|
|
rotmat.SetColumn2(v);
|
|
rotmat.SetColumn3(s);
|
|
GetOrtho(3, rotmat);
|
|
}
|
|
|
|
// This final version of GetOrtho is mainly for internal use.
|
|
// It uses a Gram-Schmidt procedure to extend a partial orthonormal
|
|
// basis to a complete orthonormal basis.
|
|
// j = number of columns of rotmat that have already been set.
|
|
void GetOrtho(int j, RotationMapR4& rotmat)
|
|
{
|
|
if (j == 0)
|
|
{
|
|
rotmat.SetIdentity();
|
|
return;
|
|
}
|
|
if (j == 1)
|
|
{
|
|
rotmat.SetColumn2(-rotmat.m21, rotmat.m11, -rotmat.m41, rotmat.m31);
|
|
j = 2;
|
|
}
|
|
|
|
assert(rotmat.Column1().Norm() < 1.0001 && 0.9999 < rotmat.Column1().Norm() && rotmat.Column1().Norm() < 1.0001 && 0.9999 < rotmat.Column1().Norm() && (rotmat.Column1() ^ rotmat.Column2()) < 0.001 && (rotmat.Column1() ^ rotmat.Column2()) > -0.001);
|
|
|
|
// 2x2 subdeterminants in first 2 columns
|
|
|
|
double d12 = rotmat.m11 * rotmat.m22 - rotmat.m12 * rotmat.m21;
|
|
double d13 = rotmat.m11 * rotmat.m32 - rotmat.m12 * rotmat.m31;
|
|
double d14 = rotmat.m11 * rotmat.m42 - rotmat.m12 * rotmat.m41;
|
|
double d23 = rotmat.m21 * rotmat.m32 - rotmat.m22 * rotmat.m31;
|
|
double d24 = rotmat.m21 * rotmat.m42 - rotmat.m22 * rotmat.m41;
|
|
double d34 = rotmat.m31 * rotmat.m42 - rotmat.m32 * rotmat.m41;
|
|
VectorR4 vec3;
|
|
|
|
if (j == 2)
|
|
{
|
|
if (d12 > 0.4 || d12 < -0.4 || d13 > 0.4 || d13 < -0.4 || d23 > 0.4 || d23 < -0.4)
|
|
{
|
|
vec3.Set(d23, -d13, d12, 0.0);
|
|
}
|
|
else if (d24 > 0.4 || d24 < -0.4 || d14 > 0.4 || d14 < -0.4)
|
|
{
|
|
vec3.Set(d24, -d14, 0.0, d12);
|
|
}
|
|
else
|
|
{
|
|
vec3.Set(d34, 0.0, -d14, d13);
|
|
}
|
|
vec3.Normalize();
|
|
rotmat.SetColumn3(vec3);
|
|
}
|
|
|
|
// Do the final column
|
|
|
|
rotmat.SetColumn4(
|
|
-rotmat.m23 * d34 + rotmat.m33 * d24 - rotmat.m43 * d23,
|
|
rotmat.m13 * d34 - rotmat.m33 * d14 + rotmat.m43 * d13,
|
|
-rotmat.m13 * d24 + rotmat.m23 * d14 - rotmat.m43 * d12,
|
|
rotmat.m13 * d23 - rotmat.m23 * d13 + rotmat.m33 * d12);
|
|
|
|
assert(0.99 < (((LinearMapR4)rotmat)).Determinant() && (((LinearMapR4)rotmat)).Determinant() < 1.01);
|
|
}
|
|
|
|
// *********************************************************************
|
|
// Rotation routines *
|
|
// *********************************************************************
|
|
|
|
// 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.
|
|
VectorR4& VectorR4::RotateUnitInDirection(const VectorR4& dir)
|
|
{
|
|
assert(this->Norm() < 1.0001 && this->Norm() > 0.9999 &&
|
|
(dir ^ (*this)) < 0.0001 && (dir ^ (*this)) > -0.0001);
|
|
|
|
double theta = dir.NormSq();
|
|
if (theta == 0.0)
|
|
{
|
|
return *this;
|
|
}
|
|
else
|
|
{
|
|
theta = sqrt(theta);
|
|
double costheta = cos(theta);
|
|
double sintheta = sin(theta);
|
|
VectorR4 dirUnit = dir / theta;
|
|
*this = costheta * (*this) + sintheta * dirUnit;
|
|
// this->NormalizeFast();
|
|
return (*this);
|
|
}
|
|
}
|
|
|
|
// RotateToMap returns a RotationMapR4 that rotates fromVec to toVec,
|
|
// leaving the orthogonal subspace fixed.
|
|
// fromVec and toVec should be unit vectors
|
|
RotationMapR4 RotateToMap(const VectorR4& fromVec, const VectorR4& toVec)
|
|
{
|
|
LinearMapR4 result;
|
|
result.SetIdentity();
|
|
LinearMapR4 temp;
|
|
VectorR4 vPerp = ProjectPerpUnitDiff(toVec, fromVec);
|
|
double sintheta = vPerp.Norm(); // theta = angle between toVec and fromVec
|
|
VectorR4 vProj = toVec - vPerp;
|
|
double costheta = vProj ^ fromVec;
|
|
if (sintheta == 0.0)
|
|
{
|
|
// The vectors either coincide (return identity) or directly oppose
|
|
if (costheta < 0.0)
|
|
{
|
|
result = -result; // Vectors directly oppose: return -identity.
|
|
}
|
|
}
|
|
else
|
|
{
|
|
vPerp /= sintheta; // Normalize
|
|
VectorProjectMap(fromVec, temp); // project in fromVec direction
|
|
temp *= (costheta - 1.0);
|
|
result += temp;
|
|
VectorProjectMap(vPerp, temp); // Project in vPerp direction
|
|
temp *= (costheta - 1.0);
|
|
result += temp;
|
|
TimesTranspose(vPerp, fromVec, temp); // temp = (vPerp)*(fromVec^T)
|
|
temp *= sintheta;
|
|
result += temp;
|
|
temp.MakeTranspose();
|
|
result -= temp; // (-sintheta)*(fromVec)*(vPerp^T)
|
|
}
|
|
RotationMapR4 rotationResult;
|
|
rotationResult.Set(result); // Make explicitly a RotationMapR4
|
|
return rotationResult;
|
|
}
|
|
|
|
// ***************************************************************
|
|
// Stream Output Routines *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
ostream& operator<<(ostream& os, const VectorR4& u)
|
|
{
|
|
return (os << "<" << u.x << "," << u.y << "," << u.z << "," << u.w << ">");
|
|
}
|