mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 14:10:11 +00:00
1103 lines
29 KiB
C++
1103 lines
29 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.
|
|
*
|
|
*
|
|
*/
|
|
|
|
|
|
//
|
|
// Linear Algebra Classes over R4
|
|
//
|
|
//
|
|
// A. Vector and Position classes
|
|
//
|
|
// A.1. VectorR4: a column vector of length 4
|
|
//
|
|
// B. Matrix Classes
|
|
//
|
|
// B.1 LinearMapR4 - arbitrary linear map; 4x4 real matrix
|
|
//
|
|
// B.2 RotationMapR4 - orthonormal 4x4 matrix
|
|
//
|
|
|
|
#ifndef LINEAR_R4_H
|
|
#define LINEAR_R4_H
|
|
|
|
#include <math.h>
|
|
#include <assert.h>
|
|
#include <iostream>
|
|
#include "LinearR3.h"
|
|
using namespace std;
|
|
|
|
class VectorR4; // R4 Vector
|
|
class LinearMapR4; // 4x4 real matrix
|
|
class RotationMapR4; // 4x4 rotation map
|
|
|
|
// **************************************
|
|
// VectorR4 class *
|
|
// * * * * * * * * * * * * * * * * * * **
|
|
|
|
class VectorR4 {
|
|
|
|
public:
|
|
double x, y, z, w; // The x & y & z & w coordinates.
|
|
|
|
static const VectorR4 Zero;
|
|
static const VectorR4 UnitX;
|
|
static const VectorR4 UnitY;
|
|
static const VectorR4 UnitZ;
|
|
static const VectorR4 UnitW;
|
|
static const VectorR4 NegUnitX;
|
|
static const VectorR4 NegUnitY;
|
|
static const VectorR4 NegUnitZ;
|
|
static const VectorR4 NegUnitW;
|
|
|
|
public:
|
|
VectorR4( ) : x(0.0), y(0.0), z(0.0), w(0.0) {}
|
|
VectorR4( double xVal, double yVal, double zVal, double wVal )
|
|
: x(xVal), y(yVal), z(zVal), w(wVal) {}
|
|
VectorR4( const Quaternion& q); // Definition with Quaternion routines
|
|
|
|
VectorR4& SetZero() { x=0.0; y=0.0; z=0.0; w=0.0; return *this;}
|
|
VectorR4& Set( double xx, double yy, double zz, double ww )
|
|
{ x=xx; y=yy; z=zz; w=ww; return *this;}
|
|
VectorR4& Set ( const Quaternion& ); // Defined with Quaternion
|
|
VectorR4& Set ( const VectorHgR3& h ) {x=h.x; y=h.y; z=h.z; w=h.w; return *this; }
|
|
VectorR4& Load( const double* v );
|
|
VectorR4& Load( const float* v );
|
|
void Dump( double* v ) const;
|
|
void Dump( float* v ) const;
|
|
|
|
VectorR4& operator+= ( const VectorR4& v )
|
|
{ x+=v.x; y+=v.y; z+=v.z; w+=v.w; return(*this); }
|
|
VectorR4& operator-= ( const VectorR4& v )
|
|
{ x-=v.x; y-=v.y; z-=v.z; w-=v.w; return(*this); }
|
|
VectorR4& operator*= ( double m )
|
|
{ x*=m; y*=m; z*=m; w*=m; return(*this); }
|
|
VectorR4& operator/= ( double m )
|
|
{ double mInv = 1.0/m;
|
|
x*=mInv; y*=mInv; z*=mInv; w*=mInv;
|
|
return(*this); }
|
|
VectorR4 operator- () const { return ( VectorR4(-x, -y, -z, -w) ); }
|
|
VectorR4& ArrayProd(const VectorR4&); // Component-wise product
|
|
VectorR4& ArrayProd3(const VectorR3&); // Component-wise product
|
|
|
|
VectorR4& AddScaled( const VectorR4& u, double s );
|
|
|
|
double Norm() const { return ( (double)sqrt( x*x + y*y + z*z +w*w) ); }
|
|
double NormSq() const { return ( x*x + y*y + z*z + w*w ); }
|
|
double Dist( const VectorR4& u ) const; // Distance from u
|
|
double DistSq( const VectorR4& u ) const; // Distance from u
|
|
double MaxAbs() const;
|
|
VectorR4& Normalize () { *this /= Norm(); return *this; } // No error checking
|
|
inline VectorR4& MakeUnit(); // Normalize() with error checking
|
|
inline VectorR4& ReNormalize();
|
|
bool IsUnit( ) const
|
|
{ double norm = Norm();
|
|
return ( 1.000001>=norm && norm>=0.999999 ); }
|
|
bool IsUnit( double tolerance ) const
|
|
{ double norm = Norm();
|
|
return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); }
|
|
bool IsZero() const { return ( x==0.0 && y==0.0 && z==0.0 && w==0.0); }
|
|
bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );}
|
|
// tolerance should be non-negative
|
|
|
|
VectorR4& RotateUnitInDirection ( const VectorR4& dir); // rotate in direction dir
|
|
|
|
};
|
|
|
|
inline VectorR4 operator+( const VectorR4& u, const VectorR4& v );
|
|
inline VectorR4 operator-( const VectorR4& u, const VectorR4& v );
|
|
inline VectorR4 operator*( const VectorR4& u, double m);
|
|
inline VectorR4 operator*( double m, const VectorR4& u);
|
|
inline VectorR4 operator/( const VectorR4& u, double m);
|
|
inline int operator==( const VectorR4& u, const VectorR4& v );
|
|
|
|
inline double operator^ (const VectorR4& u, const VectorR4& v ); // Dot Product
|
|
inline VectorR4 ArrayProd(const VectorR4& u, const VectorR4& v );
|
|
|
|
inline double Mag(const VectorR4& u) { return u.Norm(); }
|
|
inline double Dist(const VectorR4& u, const VectorR4& v) { return u.Dist(v); }
|
|
inline double DistSq(const VectorR4& u, const VectorR4& v) { return u.DistSq(v); }
|
|
inline double NormalizeError (const VectorR4& u);
|
|
|
|
// ********************************************************************
|
|
// Matrix4x4 - base class for 4x4 matrices *
|
|
// * * * * * * * * * * * * * * * * * * * * * **************************
|
|
|
|
class Matrix4x4 {
|
|
|
|
public:
|
|
double m11, m12, m13, m14, m21, m22, m23, m24,
|
|
m31, m32, m33, m34, m41, m42, m43, m44;
|
|
|
|
// Implements a 4x4 matrix: m_i_j - row-i and column-j entry
|
|
|
|
static const Matrix4x4 Identity;
|
|
|
|
|
|
public:
|
|
|
|
Matrix4x4();
|
|
Matrix4x4( const VectorR4&, const VectorR4&,
|
|
const VectorR4&, const VectorR4& ); // Sets by columns!
|
|
Matrix4x4( double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double ); // Sets by columns
|
|
|
|
inline void SetIdentity (); // Set to the identity map
|
|
inline void SetZero (); // Set to the zero map
|
|
inline void Set ( const Matrix4x4& ); // Set to the matrix.
|
|
inline void Set( const VectorR4&, const VectorR4&,
|
|
const VectorR4&, const VectorR4& );
|
|
inline void Set( double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double );
|
|
inline void SetByRows( const VectorR4&, const VectorR4&,
|
|
const VectorR4&, const VectorR4& );
|
|
inline void SetByRows( double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double );
|
|
inline void SetColumn1 ( double, double, double, double );
|
|
inline void SetColumn2 ( double, double, double, double );
|
|
inline void SetColumn3 ( double, double, double, double );
|
|
inline void SetColumn4 ( double, double, double, double );
|
|
inline void SetColumn1 ( const VectorR4& );
|
|
inline void SetColumn2 ( const VectorR4& );
|
|
inline void SetColumn3 ( const VectorR4& );
|
|
inline void SetColumn4 ( const VectorR4& );
|
|
inline VectorR4 Column1() const;
|
|
inline VectorR4 Column2() const;
|
|
inline VectorR4 Column3() const;
|
|
inline VectorR4 Column4() const;
|
|
|
|
inline void SetDiagonal( double, double, double, double );
|
|
inline void SetDiagonal( const VectorR4& );
|
|
inline double Diagonal( int );
|
|
|
|
inline void MakeTranspose(); // Transposes it.
|
|
void operator*= (const Matrix4x4& B); // Matrix product
|
|
|
|
Matrix4x4& ReNormalize();
|
|
};
|
|
|
|
inline VectorR4 operator* ( const Matrix4x4&, const VectorR4& );
|
|
|
|
ostream& operator<< ( ostream& os, const Matrix4x4& A );
|
|
|
|
|
|
// *****************************************
|
|
// LinearMapR4 class *
|
|
// * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
class LinearMapR4 : public Matrix4x4 {
|
|
|
|
public:
|
|
|
|
LinearMapR4();
|
|
LinearMapR4( const VectorR4&, const VectorR4&,
|
|
const VectorR4&, const VectorR4& ); // Sets by columns!
|
|
LinearMapR4( double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double ); // Sets by columns
|
|
LinearMapR4 ( const Matrix4x4& );
|
|
|
|
inline LinearMapR4& operator+= (const LinearMapR4& );
|
|
inline LinearMapR4& operator-= (const LinearMapR4& );
|
|
inline LinearMapR4& operator*= (double);
|
|
inline LinearMapR4& operator/= (double);
|
|
inline LinearMapR4& operator*= (const Matrix4x4& ); // Matrix product
|
|
|
|
inline LinearMapR4 Transpose() const;
|
|
double Determinant () const; // Returns the determinant
|
|
LinearMapR4 Inverse() const; // Returns inverse
|
|
LinearMapR4& Invert(); // Converts into inverse.
|
|
VectorR4 Solve(const VectorR4&) const; // Returns solution
|
|
LinearMapR4 PseudoInverse() const; // Returns pseudo-inverse TO DO
|
|
VectorR4 PseudoSolve(const VectorR4&); // Finds least squares solution TO DO
|
|
};
|
|
|
|
inline LinearMapR4 operator+ (const LinearMapR4&, const LinearMapR4&);
|
|
inline LinearMapR4 operator- (const LinearMapR4&);
|
|
inline LinearMapR4 operator- (const LinearMapR4&, const LinearMapR4&);
|
|
inline LinearMapR4 operator* ( const LinearMapR4&, double);
|
|
inline LinearMapR4 operator* ( double, const LinearMapR4& );
|
|
inline LinearMapR4 operator/ ( const LinearMapR4&, double );
|
|
inline LinearMapR4 operator* ( const Matrix4x4&, const LinearMapR4& );
|
|
inline LinearMapR4 operator* ( const LinearMapR4&, const Matrix4x4& );
|
|
// Matrix product (composition)
|
|
|
|
|
|
// *******************************************
|
|
// RotationMapR4 class *
|
|
// * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
class RotationMapR4 : public Matrix4x4 {
|
|
|
|
public:
|
|
|
|
RotationMapR4();
|
|
RotationMapR4( const VectorR4&, const VectorR4&,
|
|
const VectorR4&, const VectorR4& ); // Sets by columns!
|
|
RotationMapR4( double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double,
|
|
double, double, double, double ); // Sets by columns!
|
|
|
|
RotationMapR4& SetZero(); // IT IS AN ERROR TO USE THIS FUNCTION!
|
|
|
|
inline RotationMapR4& operator*= (const RotationMapR4& ); // Matrix product
|
|
|
|
inline RotationMapR4 Transpose() const;
|
|
inline RotationMapR4 Inverse() const { return Transpose(); }; // Returns the transpose
|
|
inline RotationMapR4& Invert() { MakeTranspose(); return *this; }; // Transposes it.
|
|
inline VectorR4 Invert(const VectorR4&) const; // Returns solution
|
|
};
|
|
|
|
inline RotationMapR4 operator* ( const RotationMapR4&, const RotationMapR4& );
|
|
// Matrix product (composition)
|
|
|
|
|
|
// ***************************************************************
|
|
// * 4-space vector and matrix utilities (prototypes) *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
// Returns the angle between vectors u and v.
|
|
// Use SolidAngleUnit if both vectors are unit vectors
|
|
inline double SolidAngle( const VectorR4& u, const VectorR4& v);
|
|
inline double SolidAngleUnit( const VectorR4 u, const VectorR4 v );
|
|
|
|
// Returns a righthanded orthonormal basis to complement vectors u,v,w.
|
|
// The vectors u,v,w must be unit and orthonormal.
|
|
void GetOrtho( const VectorR4& u, RotationMapR4& rotmap );
|
|
void GetOrtho( const VectorR4& u, const VectorR4& v, RotationMapR4& rotmap );
|
|
void GetOrtho( const VectorR4& u, const VectorR4& v, const VectorR4& w,
|
|
RotationMapR4& rotmap );
|
|
void GetOrtho( int j, RotationMapR4& rotmap); // Mainly for internal use
|
|
|
|
// Projections
|
|
|
|
inline VectorR4 ProjectToUnit ( const VectorR4& u, const VectorR4& v);
|
|
// Project u onto v
|
|
inline VectorR4 ProjectPerpUnit ( const VectorR4& u, const VectorR4 & v);
|
|
// Project perp to v
|
|
inline VectorR4 ProjectPerpUnitDiff ( const VectorR4& u, const VectorR4& v);
|
|
// v must be a unit vector.
|
|
|
|
// Returns the projection of u onto unit v
|
|
inline VectorR4 ProjectToUnit ( const VectorR4& u, const VectorR4& v)
|
|
{
|
|
return (u^v)*v;
|
|
}
|
|
|
|
// Returns the projection of u onto the plane perpindicular to the unit vector v
|
|
inline VectorR4 ProjectPerpUnit ( const VectorR4& u, const VectorR4& v)
|
|
{
|
|
return ( u - ((u^v)*v) );
|
|
}
|
|
|
|
// Returns the projection of u onto the plane perpindicular to the unit vector v
|
|
// This one is more stable when u and v are nearly equal.
|
|
inline VectorR4 ProjectPerpUnitDiff ( const VectorR4& u, const VectorR4& v)
|
|
{
|
|
VectorR4 ans = u;
|
|
ans -= v;
|
|
ans -= ((ans^v)*v);
|
|
return ans; // ans = (u-v) - ((u-v)^v)*v
|
|
}
|
|
|
|
|
|
// Projection maps (LinearMapR4's)
|
|
|
|
// VectorProjectMap returns map projecting onto a given vector u.
|
|
// u should be a unit vector (otherwise the returned map is
|
|
// scaled according to the magnitude of u.
|
|
inline void VectorProjectMap( const VectorR4& u, LinearMapR4& M )
|
|
{
|
|
double a = u.x*u.y;
|
|
double b = u.x*u.z;
|
|
double c = u.x*u.w;
|
|
double d = u.y*u.z;
|
|
double e = u.y*u.w;
|
|
double f = u.z*u.w;
|
|
M.Set( u.x*u.x, a, b, c,
|
|
a, u.y*u.y, d, e,
|
|
b, d, u.z*u.z, f,
|
|
c, e, f, u.w*u.w );
|
|
}
|
|
|
|
inline LinearMapR4 VectorProjectMap( const VectorR4& u )
|
|
{
|
|
LinearMapR4 result;
|
|
VectorProjectMap( u, result );
|
|
return result;
|
|
}
|
|
|
|
inline LinearMapR4 PerpProjectMap ( const VectorR4& u );
|
|
// u - must be unit vector.
|
|
|
|
LinearMapR4 TimesTranspose( const VectorR4& u, const VectorR4& v); // u * v^T.
|
|
inline void TimesTranspose( const VectorR4& u, const VectorR4& v, LinearMapR4& M);
|
|
|
|
// Rotation Maps
|
|
|
|
RotationMapR4 RotateToMap( const VectorR4& fromVec, const VectorR4& toVec);
|
|
// fromVec and toVec should be unit vectors
|
|
|
|
|
|
|
|
// ***************************************************************
|
|
// * Stream Output Routines (Prototypes) *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
ostream& operator<< ( ostream& os, const VectorR4& u );
|
|
|
|
|
|
// *****************************************************
|
|
// * VectorR4 class - inlined functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
inline VectorR4& VectorR4::Load( const double* v )
|
|
{
|
|
x = *v;
|
|
y = *(v+1);
|
|
z = *(v+2);
|
|
w = *(v+3);
|
|
return *this;
|
|
}
|
|
|
|
inline VectorR4& VectorR4::Load( const float* v )
|
|
{
|
|
x = *v;
|
|
y = *(v+1);
|
|
z = *(v+2);
|
|
w = *(v+3);
|
|
return *this;
|
|
}
|
|
|
|
inline void VectorR4::Dump( double* v ) const
|
|
{
|
|
*v = x;
|
|
*(v+1) = y;
|
|
*(v+2) = z;
|
|
*(v+3) = w;
|
|
}
|
|
|
|
inline void VectorR4::Dump( float* v ) const
|
|
{
|
|
*v = (float)x;
|
|
*(v+1) = (float)y;
|
|
*(v+2) = (float)z;
|
|
*(v+3) = (float)w;
|
|
}
|
|
|
|
inline VectorR4& VectorR4::MakeUnit () // Convert to unit vector (or leave zero).
|
|
{
|
|
double nSq = NormSq();
|
|
if (nSq != 0.0) {
|
|
*this /= sqrt(nSq);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
inline VectorR4 operator+( const VectorR4& u, const VectorR4& v )
|
|
{
|
|
return VectorR4(u.x+v.x, u.y+v.y, u.z+v.z, u.w+v.w );
|
|
}
|
|
inline VectorR4 operator-( const VectorR4& u, const VectorR4& v )
|
|
{
|
|
return VectorR4(u.x-v.x, u.y-v.y, u.z-v.z, u.w-v.w);
|
|
}
|
|
inline VectorR4 operator*( const VectorR4& u, double m)
|
|
{
|
|
return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m );
|
|
}
|
|
inline VectorR4 operator*( double m, const VectorR4& u)
|
|
{
|
|
return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m );
|
|
}
|
|
inline VectorR4 operator/( const VectorR4& u, double m)
|
|
{
|
|
double mInv = 1.0/m;
|
|
return VectorR4( u.x*mInv, u.y*mInv, u.z*mInv, u.w*mInv );
|
|
}
|
|
|
|
inline int operator==( const VectorR4& u, const VectorR4& v )
|
|
{
|
|
return ( u.x==v.x && u.y==v.y && u.z==v.z && u.w==v.w );
|
|
}
|
|
|
|
inline double operator^ ( const VectorR4& u, const VectorR4& v ) // Dot Product
|
|
{
|
|
return ( u.x*v.x + u.y*v.y + u.z*v.z + u.w*v.w );
|
|
}
|
|
|
|
inline VectorR4 ArrayProd ( const VectorR4& u, const VectorR4& v )
|
|
{
|
|
return ( VectorR4( u.x*v.x, u.y*v.y, u.z*v.z, u.w*v.w ) );
|
|
}
|
|
|
|
inline VectorR4& VectorR4::ArrayProd (const VectorR4& v) // Component-wise Product
|
|
{
|
|
x *= v.x;
|
|
y *= v.y;
|
|
z *= v.z;
|
|
w *= v.w;
|
|
return ( *this );
|
|
}
|
|
|
|
inline VectorR4& VectorR4::ArrayProd3 (const VectorR3& v) // Component-wise Product
|
|
{
|
|
x *= v.x;
|
|
y *= v.y;
|
|
z *= v.z;
|
|
return ( *this );
|
|
}
|
|
|
|
inline VectorR4& VectorR4::AddScaled( const VectorR4& u, double s )
|
|
{
|
|
x += s*u.x;
|
|
y += s*u.y;
|
|
z += s*u.z;
|
|
w += s*u.w;
|
|
return(*this);
|
|
}
|
|
|
|
inline VectorR4& VectorR4::ReNormalize() // Convert near unit back to unit
|
|
{
|
|
double nSq = NormSq();
|
|
double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor
|
|
*this *= mFact;
|
|
return *this;
|
|
}
|
|
|
|
inline double NormalizeError (const VectorR4& u)
|
|
{
|
|
double discrepancy;
|
|
discrepancy = u.x*u.x + u.y*u.y + u.z*u.z + u.w*u.w - 1.0;
|
|
if ( discrepancy < 0.0 ) {
|
|
discrepancy = -discrepancy;
|
|
}
|
|
return discrepancy;
|
|
}
|
|
|
|
inline VectorR3& VectorR3::SetFromHg(const VectorR4& v) {
|
|
double wInv = 1.0/v.w;
|
|
x = v.x*wInv;
|
|
y = v.y*wInv;
|
|
z = v.z*wInv;
|
|
return *this;
|
|
}
|
|
|
|
inline double VectorR4::Dist( const VectorR4& u ) const // Distance from u
|
|
{
|
|
return sqrt( DistSq(u) );
|
|
}
|
|
|
|
inline double VectorR4::DistSq( const VectorR4& u ) const // Distance from u
|
|
{
|
|
return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) + (z-u.z)*(z-u.z) + (w-u.w)*(w-u.w) );
|
|
}
|
|
|
|
|
|
// *********************************************************
|
|
// * Matrix4x4 class - inlined functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * *****
|
|
|
|
inline Matrix4x4::Matrix4x4() {}
|
|
|
|
inline Matrix4x4::Matrix4x4( const VectorR4& u, const VectorR4& v,
|
|
const VectorR4& s, const VectorR4& t)
|
|
{
|
|
m11 = u.x; // Column 1
|
|
m21 = u.y;
|
|
m31 = u.z;
|
|
m41 = u.w;
|
|
m12 = v.x; // Column 2
|
|
m22 = v.y;
|
|
m32 = v.z;
|
|
m42 = v.w;
|
|
m13 = s.x; // Column 3
|
|
m23 = s.y;
|
|
m33 = s.z;
|
|
m43 = s.w;
|
|
m14 = t.x; // Column 4
|
|
m24 = t.y;
|
|
m34 = t.z;
|
|
m44 = t.w;
|
|
}
|
|
|
|
inline Matrix4x4::Matrix4x4( double a11, double a21, double a31, double a41,
|
|
double a12, double a22, double a32, double a42,
|
|
double a13, double a23, double a33, double a43,
|
|
double a14, double a24, double a34, double a44)
|
|
// Values specified in column order!!!
|
|
{
|
|
m11 = a11; // Row 1
|
|
m12 = a12;
|
|
m13 = a13;
|
|
m14 = a14;
|
|
m21 = a21; // Row 2
|
|
m22 = a22;
|
|
m23 = a23;
|
|
m24 = a24;
|
|
m31 = a31; // Row 3
|
|
m32 = a32;
|
|
m33 = a33;
|
|
m34 = a34;
|
|
m41 = a41; // Row 4
|
|
m42 = a42;
|
|
m43 = a43;
|
|
m44 = a44;
|
|
}
|
|
|
|
/*
|
|
inline Matrix4x4::Matrix4x4 ( const Matrix4x4& A)
|
|
: m11(A.m11), m12(A.m12), m13(A.m13), m14(A.m14),
|
|
m21(A.m21), m22(A.m22), m23(A.m23), m24(A.m24),
|
|
m31(A.m31), m32(A.m32), m33(A.m33), m34(A.m34),
|
|
m41(A.m41), m42(A.m42), m43(A.m43), m44(A.m44) {} */
|
|
|
|
inline void Matrix4x4::SetIdentity ( )
|
|
{
|
|
m12 = m13 = m14 =
|
|
m21 = m23 = m24 =
|
|
m31 = m32 = m34 =
|
|
m41 = m42 = m43 = 0.0;
|
|
m11 = m22 = m33 = m44 = 1.0;
|
|
}
|
|
|
|
inline void Matrix4x4::Set( const VectorR4& u, const VectorR4& v,
|
|
const VectorR4& s, const VectorR4& t )
|
|
{
|
|
m11 = u.x; // Column 1
|
|
m21 = u.y;
|
|
m31 = u.z;
|
|
m41 = u.w;
|
|
m12 = v.x; // Column 2
|
|
m22 = v.y;
|
|
m32 = v.z;
|
|
m42 = v.w;
|
|
m13 = s.x; // Column 3
|
|
m23 = s.y;
|
|
m33 = s.z;
|
|
m43 = s.w;
|
|
m14 = t.x; // Column 4
|
|
m24 = t.y;
|
|
m34 = t.z;
|
|
m44 = t.w;
|
|
}
|
|
|
|
inline void Matrix4x4::Set( double a11, double a21, double a31, double a41,
|
|
double a12, double a22, double a32, double a42,
|
|
double a13, double a23, double a33, double a43,
|
|
double a14, double a24, double a34, double a44)
|
|
// Values specified in column order!!!
|
|
{
|
|
m11 = a11; // Row 1
|
|
m12 = a12;
|
|
m13 = a13;
|
|
m14 = a14;
|
|
m21 = a21; // Row 2
|
|
m22 = a22;
|
|
m23 = a23;
|
|
m24 = a24;
|
|
m31 = a31; // Row 3
|
|
m32 = a32;
|
|
m33 = a33;
|
|
m34 = a34;
|
|
m41 = a41; // Row 4
|
|
m42 = a42;
|
|
m43 = a43;
|
|
m44 = a44;
|
|
}
|
|
|
|
inline void Matrix4x4::Set ( const Matrix4x4& M ) // Set to the matrix.
|
|
{
|
|
m11 = M.m11;
|
|
m12 = M.m12;
|
|
m13 = M.m13;
|
|
m14 = M.m14;
|
|
m21 = M.m21;
|
|
m22 = M.m22;
|
|
m23 = M.m23;
|
|
m24 = M.m24;
|
|
m31 = M.m31;
|
|
m32 = M.m32;
|
|
m33 = M.m33;
|
|
m34 = M.m34;
|
|
m41 = M.m41;
|
|
m42 = M.m42;
|
|
m43 = M.m43;
|
|
m44 = M.m44;
|
|
}
|
|
|
|
inline void Matrix4x4::SetZero( )
|
|
{
|
|
m11 = m12 = m13 = m14 = m21 = m22 = m23 = m24
|
|
= m31 = m32 = m33 = m34 = m41 = m42 = m43 = m44 = 0.0;
|
|
}
|
|
|
|
inline void Matrix4x4::SetByRows( const VectorR4& u, const VectorR4& v,
|
|
const VectorR4& s, const VectorR4& t )
|
|
{
|
|
m11 = u.x; // Row 1
|
|
m12 = u.y;
|
|
m13 = u.z;
|
|
m14 = u.w;
|
|
m21 = v.x; // Row 2
|
|
m22 = v.y;
|
|
m23 = v.z;
|
|
m24 = v.w;
|
|
m31 = s.x; // Row 3
|
|
m32 = s.y;
|
|
m33 = s.z;
|
|
m34 = s.w;
|
|
m41 = t.x; // Row 4
|
|
m42 = t.y;
|
|
m43 = t.z;
|
|
m44 = t.w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetByRows( double a11, double a12, double a13, double a14,
|
|
double a21, double a22, double a23, double a24,
|
|
double a31, double a32, double a33, double a34,
|
|
double a41, double a42, double a43, double a44 )
|
|
// Values specified in row order!!!
|
|
{
|
|
m11 = a11; // Row 1
|
|
m12 = a12;
|
|
m13 = a13;
|
|
m14 = a14;
|
|
m21 = a21; // Row 2
|
|
m22 = a22;
|
|
m23 = a23;
|
|
m24 = a24;
|
|
m31 = a31; // Row 3
|
|
m32 = a32;
|
|
m33 = a33;
|
|
m34 = a34;
|
|
m41 = a41; // Row 4
|
|
m42 = a42;
|
|
m43 = a43;
|
|
m44 = a44;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn1 ( double x, double y, double z, double w)
|
|
{
|
|
m11 = x; m21 = y; m31= z; m41 = w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn2 ( double x, double y, double z, double w)
|
|
{
|
|
m12 = x; m22 = y; m32= z; m42 = w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn3 ( double x, double y, double z, double w)
|
|
{
|
|
m13 = x; m23 = y; m33= z; m43 = w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn4 ( double x, double y, double z, double w)
|
|
{
|
|
m14 = x; m24 = y; m34= z; m44 = w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn1 ( const VectorR4& u )
|
|
{
|
|
m11 = u.x; m21 = u.y; m31 = u.z; m41 = u.w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn2 ( const VectorR4& u )
|
|
{
|
|
m12 = u.x; m22 = u.y; m32 = u.z; m42 = u.w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn3 ( const VectorR4& u )
|
|
{
|
|
m13 = u.x; m23 = u.y; m33 = u.z; m43 = u.w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetColumn4 ( const VectorR4& u )
|
|
{
|
|
m14 = u.x; m24 = u.y; m34 = u.z; m44 = u.w;
|
|
}
|
|
|
|
VectorR4 Matrix4x4::Column1() const
|
|
{
|
|
return ( VectorR4(m11, m21, m31, m41) );
|
|
}
|
|
|
|
VectorR4 Matrix4x4::Column2() const
|
|
{
|
|
return ( VectorR4(m12, m22, m32, m42) );
|
|
}
|
|
|
|
VectorR4 Matrix4x4::Column3() const
|
|
{
|
|
return ( VectorR4(m13, m23, m33, m43) );
|
|
}
|
|
|
|
VectorR4 Matrix4x4::Column4() const
|
|
{
|
|
return ( VectorR4(m14, m24, m34, m44) );
|
|
}
|
|
|
|
inline void Matrix4x4::SetDiagonal( double x, double y,
|
|
double z, double w)
|
|
{
|
|
m11 = x;
|
|
m22 = y;
|
|
m33 = z;
|
|
m44 = w;
|
|
}
|
|
|
|
inline void Matrix4x4::SetDiagonal( const VectorR4& u )
|
|
{
|
|
SetDiagonal ( u.x, u.y, u.z, u.w );
|
|
}
|
|
|
|
inline double Matrix4x4::Diagonal( int i )
|
|
{
|
|
switch (i) {
|
|
case 0:
|
|
return m11;
|
|
case 1:
|
|
return m22;
|
|
case 2:
|
|
return m33;
|
|
case 3:
|
|
return m44;
|
|
default:
|
|
assert(0);
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
inline void Matrix4x4::MakeTranspose() // Transposes it.
|
|
{
|
|
double temp;
|
|
temp = m12;
|
|
m12 = m21;
|
|
m21=temp;
|
|
temp = m13;
|
|
m13 = m31;
|
|
m31 = temp;
|
|
temp = m14;
|
|
m14 = m41;
|
|
m41 = temp;
|
|
temp = m23;
|
|
m23 = m32;
|
|
m32 = temp;
|
|
temp = m24;
|
|
m24 = m42;
|
|
m42 = temp;
|
|
temp = m34;
|
|
m34 = m43;
|
|
m43 = temp;
|
|
}
|
|
|
|
inline VectorR4 operator* ( const Matrix4x4& A, const VectorR4& u)
|
|
{
|
|
VectorR4 ret;
|
|
ret.x = A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14*u.w;
|
|
ret.y = A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24*u.w;
|
|
ret.z = A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34*u.w;
|
|
ret.w = A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44*u.w;
|
|
return ret;
|
|
}
|
|
|
|
|
|
// ******************************************************
|
|
// * LinearMapR4 class - inlined functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
inline LinearMapR4::LinearMapR4()
|
|
{
|
|
SetZero();
|
|
return;
|
|
}
|
|
|
|
inline LinearMapR4::LinearMapR4( const VectorR4& u, const VectorR4& v,
|
|
const VectorR4& s, const VectorR4& t)
|
|
:Matrix4x4 ( u, v, s ,t )
|
|
{ }
|
|
|
|
inline LinearMapR4::LinearMapR4(
|
|
double a11, double a21, double a31, double a41,
|
|
double a12, double a22, double a32, double a42,
|
|
double a13, double a23, double a33, double a43,
|
|
double a14, double a24, double a34, double a44)
|
|
// Values specified in column order!!!
|
|
:Matrix4x4 ( a11, a21, a31, a41, a12, a22, a32, a42,
|
|
a13, a23, a33, a43, a14, a24, a34, a44 )
|
|
{ }
|
|
|
|
inline LinearMapR4::LinearMapR4 ( const Matrix4x4& A )
|
|
: Matrix4x4 (A)
|
|
{}
|
|
|
|
|
|
inline LinearMapR4& LinearMapR4::operator+= (const LinearMapR4& B)
|
|
{
|
|
m11 += B.m11;
|
|
m12 += B.m12;
|
|
m13 += B.m13;
|
|
m14 += B.m14;
|
|
m21 += B.m21;
|
|
m22 += B.m22;
|
|
m23 += B.m23;
|
|
m24 += B.m24;
|
|
m31 += B.m31;
|
|
m32 += B.m32;
|
|
m33 += B.m33;
|
|
m34 += B.m34;
|
|
m41 += B.m41;
|
|
m42 += B.m42;
|
|
m43 += B.m43;
|
|
m44 += B.m44;
|
|
return ( *this );
|
|
}
|
|
|
|
inline LinearMapR4& LinearMapR4::operator-= (const LinearMapR4& B)
|
|
{
|
|
m11 -= B.m11;
|
|
m12 -= B.m12;
|
|
m13 -= B.m13;
|
|
m14 -= B.m14;
|
|
m21 -= B.m21;
|
|
m22 -= B.m22;
|
|
m23 -= B.m23;
|
|
m24 -= B.m24;
|
|
m31 -= B.m31;
|
|
m32 -= B.m32;
|
|
m33 -= B.m33;
|
|
m34 -= B.m34;
|
|
m41 -= B.m41;
|
|
m42 -= B.m42;
|
|
m43 -= B.m43;
|
|
m44 -= B.m44;
|
|
return( *this );
|
|
}
|
|
|
|
inline LinearMapR4 operator+ (const LinearMapR4& A, const LinearMapR4& B)
|
|
{
|
|
return( LinearMapR4( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, A.m41+B.m41,
|
|
A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, A.m42+B.m42,
|
|
A.m13+B.m13, A.m23+B.m23, A.m33+B.m33, A.m43+B.m43,
|
|
A.m14+B.m14, A.m24+B.m24, A.m34+B.m34, A.m44+B.m44) );
|
|
}
|
|
|
|
inline LinearMapR4 operator- (const LinearMapR4& A)
|
|
{
|
|
return( LinearMapR4( -A.m11, -A.m21, -A.m31, -A.m41,
|
|
-A.m12, -A.m22, -A.m32, -A.m42,
|
|
-A.m13, -A.m23, -A.m33, -A.m43,
|
|
-A.m14, -A.m24, -A.m34, -A.m44 ) );
|
|
}
|
|
|
|
inline LinearMapR4 operator- (const LinearMapR4& A, const LinearMapR4& B)
|
|
{
|
|
return( LinearMapR4( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, A.m41-B.m41,
|
|
A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, A.m42-B.m42,
|
|
A.m13-B.m13, A.m23-B.m23, A.m33-B.m33, A.m43-B.m43,
|
|
A.m14-B.m14, A.m24-B.m24, A.m34-B.m34, A.m44-B.m44 ) );
|
|
}
|
|
|
|
inline LinearMapR4& LinearMapR4::operator*= ( double b)
|
|
{
|
|
m11 *= b;
|
|
m12 *= b;
|
|
m13 *= b;
|
|
m14 *= b;
|
|
m21 *= b;
|
|
m22 *= b;
|
|
m23 *= b;
|
|
m24 *= b;
|
|
m31 *= b;
|
|
m32 *= b;
|
|
m33 *= b;
|
|
m34 *= b;
|
|
m41 *= b;
|
|
m42 *= b;
|
|
m43 *= b;
|
|
m44 *= b;
|
|
return ( *this);
|
|
}
|
|
|
|
inline LinearMapR4 operator* ( const LinearMapR4& A, double b)
|
|
{
|
|
return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b,
|
|
A.m12*b, A.m22*b, A.m32*b, A.m42*b,
|
|
A.m13*b, A.m23*b, A.m33*b, A.m43*b,
|
|
A.m14*b, A.m24*b, A.m34*b, A.m44*b) );
|
|
}
|
|
|
|
inline LinearMapR4 operator* ( double b, const LinearMapR4& A)
|
|
{
|
|
return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b,
|
|
A.m12*b, A.m22*b, A.m32*b, A.m42*b,
|
|
A.m13*b, A.m23*b, A.m33*b, A.m43*b,
|
|
A.m14*b, A.m24*b, A.m34*b, A.m44*b ) );
|
|
}
|
|
|
|
inline LinearMapR4 operator/ ( const LinearMapR4& A, double b)
|
|
{
|
|
double bInv = 1.0/b;
|
|
return ( A*bInv );
|
|
}
|
|
|
|
inline LinearMapR4& LinearMapR4::operator/= ( double b)
|
|
{
|
|
double bInv = 1.0/b;
|
|
return ( *this *= bInv );
|
|
}
|
|
|
|
inline VectorR4 operator* ( const LinearMapR4& A, const VectorR4& u)
|
|
{
|
|
return(VectorR4 ( A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14*u.w,
|
|
A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24*u.w,
|
|
A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34*u.w,
|
|
A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44*u.w ) );
|
|
}
|
|
|
|
inline LinearMapR4 LinearMapR4::Transpose() const // Returns the transpose
|
|
{
|
|
return (LinearMapR4( m11, m12, m13, m14,
|
|
m21, m22, m23, m24,
|
|
m31, m32, m33, m34,
|
|
m41, m42, m43, m44 ) );
|
|
}
|
|
|
|
inline LinearMapR4& LinearMapR4::operator*= (const Matrix4x4& B) // Matrix product
|
|
{
|
|
(*this).Matrix4x4::operator*=(B);
|
|
|
|
return( *this );
|
|
}
|
|
|
|
inline LinearMapR4 operator* ( const LinearMapR4& A, const Matrix4x4& B)
|
|
{
|
|
LinearMapR4 AA(A);
|
|
AA.Matrix4x4::operator*=(B);
|
|
return AA;
|
|
}
|
|
|
|
inline LinearMapR4 operator* ( const Matrix4x4& A, const LinearMapR4& B)
|
|
{
|
|
LinearMapR4 AA(A);
|
|
AA.Matrix4x4::operator*=(B);
|
|
return AA;
|
|
}
|
|
|
|
|
|
|
|
// ******************************************************
|
|
// * RotationMapR4 class - inlined functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
inline RotationMapR4::RotationMapR4()
|
|
{
|
|
SetIdentity();
|
|
return;
|
|
}
|
|
|
|
inline RotationMapR4::RotationMapR4( const VectorR4& u, const VectorR4& v,
|
|
const VectorR4& s, const VectorR4& t)
|
|
:Matrix4x4 ( u, v, s ,t )
|
|
{ }
|
|
|
|
inline RotationMapR4::RotationMapR4(
|
|
double a11, double a21, double a31, double a41,
|
|
double a12, double a22, double a32, double a42,
|
|
double a13, double a23, double a33, double a43,
|
|
double a14, double a24, double a34, double a44)
|
|
// Values specified in column order!!!
|
|
:Matrix4x4 ( a11, a21, a31, a41, a12, a22, a32, a42,
|
|
a13, a23, a33, a43, a14, a24, a34, a44 )
|
|
{ }
|
|
|
|
inline RotationMapR4 RotationMapR4::Transpose() const // Returns the transpose
|
|
{
|
|
return ( RotationMapR4( m11, m12, m13, m14,
|
|
m21, m22, m23, m24,
|
|
m31, m32, m33, m34,
|
|
m41, m42, m43, m44 ) );
|
|
}
|
|
|
|
inline VectorR4 RotationMapR4::Invert(const VectorR4& u) const // Returns solution
|
|
{
|
|
return (VectorR4( m11*u.x + m21*u.y + m31*u.z + m41*u.w,
|
|
m12*u.x + m22*u.y + m32*u.z + m42*u.w,
|
|
m13*u.x + m23*u.y + m33*u.z + m43*u.w,
|
|
m14*u.x + m24*u.y + m34*u.z + m44*u.w ) );
|
|
}
|
|
|
|
inline RotationMapR4& RotationMapR4::operator*= (const RotationMapR4& B) // Matrix product
|
|
{
|
|
(*this).Matrix4x4::operator*=(B);
|
|
|
|
return( *this );
|
|
}
|
|
|
|
inline RotationMapR4 operator* ( const RotationMapR4& A, const RotationMapR4& B)
|
|
{
|
|
RotationMapR4 AA(A);
|
|
AA.Matrix4x4::operator*=(B);
|
|
return AA;
|
|
}
|
|
|
|
|
|
// ***************************************************************
|
|
// * 4-space vector and matrix utilities (inlined functions) *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
inline void TimesTranspose( const VectorR4& u, const VectorR4& v, LinearMapR4& M)
|
|
{
|
|
M.Set ( v.x*u.x, v.x*u.y, v.x*u.z, v.x*u.w, // Set by columns!
|
|
v.y*u.x, v.y*u.y, v.y*u.z, v.y*u.w,
|
|
v.z*u.x, v.z*u.y, v.z*u.z, v.z*u.w,
|
|
v.w*u.x, v.w*u.y, v.w*u.z, v.w*u.w ) ;
|
|
}
|
|
|
|
// Returns the solid angle between vectors u and v (not necessarily unit vectors)
|
|
inline double SolidAngle( const VectorR4& u, const VectorR4& v)
|
|
{
|
|
double nSqU = u.NormSq();
|
|
double nSqV = v.NormSq();
|
|
if ( nSqU==0.0 && nSqV==0.0 ) {
|
|
return (0.0);
|
|
}
|
|
else {
|
|
return ( SolidAngleUnit( u/sqrt(nSqU), v/sqrt(nSqV) ) );
|
|
}
|
|
}
|
|
|
|
inline double SolidAngleUnit( const VectorR4 u, const VectorR4 v )
|
|
{
|
|
return ( atan2 ( ProjectPerpUnit(v,u).Norm(), u^v ) );
|
|
}
|
|
|
|
|
|
#endif // LINEAR_R4_H
|