TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-15 00:24:40 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

* Adjustment: Update Bullet version to 3.24.

Browse source
This commit is contained in:
Robert MacGregor 2022-06-27 10:01:08 -04:00
parent 35de012ee7
commit 4a3f31df2a
6148 changed files with 2112532 additions and 56873 deletions
Download patch file Download diff file Expand all files Collapse all files

15
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/CMakeLists.txt Normal file
View file

@ -0,0 +1,15 @@
INCLUDE_DIRECTORIES(
.
)
FILE(GLOB BussIK_SRCS "*.cpp" )
FILE(GLOB BussIK_HDRS "*.h" )
ADD_LIBRARY(BussIK ${BussIK_SRCS} ${BussIK_HDRS})
INSTALL(TARGETS BussIK
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib${LIB_SUFFIX}
ARCHIVE DESTINATION lib${LIB_SUFFIX})

790
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Jacobian.cpp Normal file
View file

@ -0,0 +1,790 @@
/*
*
* Inverse Kinematics software, with several solvers including
* Selectively Damped Least Squares Method
* Damped Least Squares Method
* Pure Pseudoinverse Method
* Jacobian Transpose Method
*
*
* Author: Samuel R. Buss, sbuss@ucsd.edu.
* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
*
*
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 <stdlib.h>
#include <math.h>
#include <assert.h>
#include <iostream>
using namespace std;
#include "Jacobian.h"
void Arrow(const VectorR3& tail, const VectorR3& head);
//extern RestPositionOn;
//extern VectorR3 target1[];
// Optimal damping values have to be determined in an ad hoc manner (Yuck!)
const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter)
//const double Jacobian::DefaultDampingLambda = 1.1; // Optimal for the DLS "double Y" shape (any lower gives jitter)
// const double Jacobian::DefaultDampingLambda = 0.7; // Optimal for the DLS "double Y" shape with distance clamping (lower gives jitter)
const double Jacobian::PseudoInverseThresholdFactor = 0.01;
const double Jacobian::MaxAngleJtranspose = 30.0 * DegreesToRadians;
const double Jacobian::MaxAnglePseudoinverse = 5.0 * DegreesToRadians;
const double Jacobian::MaxAngleDLS = 45.0 * DegreesToRadians;
const double Jacobian::MaxAngleSDLS = 45.0 * DegreesToRadians;
const double Jacobian::BaseMaxTargetDist = 0.4;
Jacobian::Jacobian(Tree* tree)
{
m_tree = tree;
m_nEffector = tree->GetNumEffector();
nJoint = tree->GetNumJoint();
nRow = 3 * m_nEffector; // Include only the linear part
nCol = nJoint;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetZero();
SetJendActive();
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w.SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dPreTheta.SetLength(nCol);
// Used by Jacobian transpose method & DLS & SDLS
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
// Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow);
dSclamp.SetLength(m_nEffector);
errorArray.SetLength(m_nEffector);
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
Jacobian::Jacobian(bool useAngularJacobian, int nDof, int numEndEffectors)
{
m_tree = 0;
m_nEffector = numEndEffectors;
if (useAngularJacobian)
{
nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
}
else
{
nRow = 3 * m_nEffector; // Include only the linear part
}
nCol = nDof;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetZero();
SetJendActive();
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w.SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dPreTheta.SetLength(nCol);
// Used by Jacobian transpose method & DLS & SDLS
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
// Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow);
dSclamp.SetLength(m_nEffector);
errorArray.SetLength(m_nEffector);
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
void Jacobian::Reset()
{
// Used by Damped Least Squares Method
DampingLambda = DefaultDampingLambda;
DampingLambdaSq = Square(DampingLambda);
// DampingLambdaSDLS = 1.5*DefaultDampingLambda;
dSclamp.Fill(HUGE_VAL);
}
// Compute the deltaS vector, dS, (the error in end effector positions
// Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian(VectorR3* targets)
{
if (m_tree == 0)
return;
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
temp = targetPos;
temp -= n->GetS();
dS.SetTriple(i, temp);
// Find all ancestors (they will usually all be joints)
// Set the corresponding entries in the Jacobians J, K.
Node* m = m_tree->GetParent(n);
while (m)
{
int j = m->GetJointNum();
assert(0 <= i && i < m_nEffector && 0 <= j && j < nJoint);
if (m->IsFrozen())
{
Jend.SetTriple(i, j, VectorR3::Zero);
Jtarget.SetTriple(i, j, VectorR3::Zero);
}
else
{
temp = m->GetS(); // joint pos.
temp -= n->GetS(); // -(end effector pos. - joint pos.)
temp *= m->GetW(); // cross product with joint rotation axis
Jend.SetTriple(i, j, temp);
temp = m->GetS(); // joint pos.
temp -= targetPos; // -(target pos. - joint pos.)
temp *= m->GetW(); // cross product with joint rotation axis
Jtarget.SetTriple(i, j, temp);
}
m = m_tree->GetParent(m);
}
}
n = m_tree->GetSuccessor(n);
}
}
void Jacobian::SetJendTrans(MatrixRmn& J)
{
Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
Jend.LoadAsSubmatrix(J);
}
void Jacobian::SetDeltaS(VectorRn& S)
{
dS.Set(S);
}
// The delta theta values have been computed in dTheta array
// Apply the delta theta values to the joints
// Nothing is done about joint limits for now.
void Jacobian::UpdateThetas()
{
// Traverse the tree to find all joints
// Update the joint angles
Node* n = m_tree->GetRoot();
while (n)
{
if (n->IsJoint())
{
int i = n->GetJointNum();
n->AddToTheta(dTheta[i]);
}
n = m_tree->GetSuccessor(n);
}
// Update the positions and rotation axes of all joints/effectors
m_tree->Compute();
}
void Jacobian::UpdateThetaDot()
{
if (m_tree == 0)
return;
// Traverse the tree to find all joints
// Update the joint angles
Node* n = m_tree->GetRoot();
while (n)
{
if (n->IsJoint())
{
int i = n->GetJointNum();
n->UpdateTheta(dTheta[i]);
}
n = m_tree->GetSuccessor(n);
}
// Update the positions and rotation axes of all joints/effectors
m_tree->Compute();
}
void Jacobian::CalcDeltaThetas(MatrixRmn& AugMat)
{
switch (CurrentUpdateMode)
{
case JACOB_Undefined:
ZeroDeltaThetas();
break;
case JACOB_JacobianTranspose:
CalcDeltaThetasTranspose();
break;
case JACOB_PseudoInverse:
CalcDeltaThetasPseudoinverse();
break;
case JACOB_DLS:
CalcDeltaThetasDLS(AugMat);
break;
case JACOB_SDLS:
CalcDeltaThetasSDLS();
break;
}
}
void Jacobian::ZeroDeltaThetas()
{
dTheta.SetZero();
}
// Find the delta theta values using inverse Jacobian method
// Uses a greedy method to decide scaling factor
void Jacobian::CalcDeltaThetasTranspose()
{
const MatrixRmn& J = ActiveJacobian();
J.MultiplyTranspose(dS, dTheta);
// Scale back the dTheta values greedily
J.Multiply(dTheta, dT1); // dT = J * dTheta
double alpha = Dot(dS, dT1) / dT1.NormSq();
assert(alpha > 0.0);
// Also scale back to be have max angle change less than MaxAngleJtranspose
double maxChange = dTheta.MaxAbs();
double beta = MaxAngleJtranspose / maxChange;
dTheta *= Min(alpha, beta);
}
void Jacobian::CalcDeltaThetasPseudoinverse()
{
MatrixRmn& J = const_cast<MatrixRmn&>(ActiveJacobian());
// Compute Singular Value Decomposition
// This an inefficient way to do Pseudoinverse, but it is convenient since we need SVD anyway
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the DLS solution.
// Delta target values are the dS values
// We multiply by Moore-Penrose pseudo-inverse of the J matrix
double pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
long diagLength = w.GetLength();
double* wPtr = w.GetPtr();
dTheta.SetZero();
for (long i = 0; i < diagLength; i++)
{
double dotProdCol = U.DotProductColumn(dS, i); // Dot product with i-th column of U
double alpha = *(wPtr++);
if (fabs(alpha) > pseudoInverseThreshold)
{
alpha = 1.0 / alpha;
MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
}
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if (maxChange > MaxAnglePseudoinverse)
{
dTheta *= MaxAnglePseudoinverse / maxChange;
}
}
void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat)
{
const MatrixRmn& J = ActiveJacobian();
MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
U.AddToDiagonal(DampingLambdaSq);
// Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
// CalcdTClampedFromdS();
// VectorRn dTextra(3*m_nEffector);
// U.Solve( dT, &dTextra );
// J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method
U.Solve(dS, &dT1, AugMat);
J.MultiplyTranspose(dT1, dTheta);
// Compute JInv in damped least square form
MatrixRmn UInv(U.GetNumRows(), U.GetNumColumns());
U.ComputeInverse(UInv);
assert(U.DebugCheckInverse(UInv));
MatrixRmn JInv(J.GetNumColumns(), J.GetNumRows());
MatrixRmn::TransposeMultiply(J, UInv, JInv);
// Compute null space projection
MatrixRmn JInvJ(J.GetNumColumns(), J.GetNumColumns());
MatrixRmn::Multiply(JInv, J, JInvJ);
MatrixRmn P(J.GetNumColumns(), J.GetNumColumns());
P.SetIdentity();
P -= JInvJ;
// Compute null space velocity
VectorRn nullV(J.GetNumColumns());
P.Multiply(desiredV, nullV);
// Compute residual
VectorRn residual(J.GetNumRows());
J.Multiply(nullV, residual);
// TODO: Use residual to set the null space term coefficient adaptively.
//printf("residual: %f\n", residual.Norm());
// Add null space velocity
dTheta += nullV;
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
void Jacobian::CalcDeltaThetasDLS(MatrixRmn& AugMat)
{
const MatrixRmn& J = ActiveJacobian();
MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
U.AddToDiagonal(DampingLambdaSq);
// Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
// CalcdTClampedFromdS();
// VectorRn dTextra(3*m_nEffector);
// U.Solve( dT, &dTextra );
// J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method
U.Solve(dS, &dT1, AugMat);
J.MultiplyTranspose(dT1, dTheta);
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat)
{
const MatrixRmn& J = ActiveJacobian();
U.SetSize(J.GetNumColumns(), J.GetNumColumns());
MatrixRmn::TransposeMultiply(J, J, U);
U.AddToDiagonal(dVec);
dT1.SetLength(J.GetNumColumns());
J.MultiplyTranspose(dS, dT1);
U.Solve(dT1, &dTheta, AugMat);
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
void Jacobian::CalcDeltaThetasDLSwithSVD()
{
const MatrixRmn& J = ActiveJacobian();
// Compute Singular Value Decomposition
// This an inefficient way to do DLS, but it is convenient since we need SVD anyway
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the DLS solution.
// Delta target values are the dS values
// We multiply by DLS inverse of the J matrix
long diagLength = w.GetLength();
double* wPtr = w.GetPtr();
dTheta.SetZero();
for (long i = 0; i < diagLength; i++)
{
double dotProdCol = U.DotProductColumn(dS, i); // Dot product with i-th column of U
double alpha = *(wPtr++);
alpha = alpha / (Square(alpha) + DampingLambdaSq);
MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
void Jacobian::CalcDeltaThetasSDLS()
{
const MatrixRmn& J = ActiveJacobian();
// Compute Singular Value Decomposition
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
int nRows = J.GetNumRows();
int numEndEffectors = m_tree->GetNumEffector();
int nCols = J.GetNumColumns();
dTheta.SetZero();
// Calculate the norms of the 3-vectors in the Jacobian
long i;
const double* jx = J.GetPtr();
double* jnx = Jnorms.GetPtr();
for (i = nCols * numEndEffectors; i > 0; i--)
{
double accumSq = Square(*(jx++));
accumSq += Square(*(jx++));
accumSq += Square(*(jx++));
*(jnx++) = sqrt(accumSq);
}
// Clamp the dS values
CalcdTClampedFromdS();
// Loop over each singular vector
for (i = 0; i < nRows; i++)
{
double wiInv = w[i];
if (NearZero(wiInv, 1.0e-10))
{
continue;
}
wiInv = 1.0 / wiInv;
double N = 0.0; // N is the quasi-1-norm of the i-th column of U
double alpha = 0.0; // alpha is the dot product of dT and the i-th column of U
const double* dTx = dT1.GetPtr();
const double* ux = U.GetColumnPtr(i);
long j;
for (j = numEndEffectors; j > 0; j--)
{
double tmp;
alpha += (*ux) * (*(dTx++));
tmp = Square(*(ux++));
alpha += (*ux) * (*(dTx++));
tmp += Square(*(ux++));
alpha += (*ux) * (*(dTx++));
tmp += Square(*(ux++));
N += sqrt(tmp);
}
// M is the quasi-1-norm of the response to angles changing according to the i-th column of V
// Then is multiplied by the wiInv value.
double M = 0.0;
double* vx = V.GetColumnPtr(i);
jnx = Jnorms.GetPtr();
for (j = nCols; j > 0; j--)
{
double accum = 0.0;
for (long k = numEndEffectors; k > 0; k--)
{
accum += *(jnx++);
}
M += fabs((*(vx++))) * accum;
}
M *= fabs(wiInv);
double gamma = MaxAngleSDLS;
if (N < M)
{
gamma *= N / M; // Scale back maximum permissable joint angle
}
// Calculate the dTheta from pure pseudoinverse considerations
double scale = alpha * wiInv; // This times i-th column of V is the psuedoinverse response
dPreTheta.LoadScaled(V.GetColumnPtr(i), scale);
// Now rescale the dTheta values.
double max = dPreTheta.MaxAbs();
double rescale = (gamma) / (gamma + max);
dTheta.AddScaled(dPreTheta, rescale);
/*if ( gamma<max) {
dTheta.AddScaled( dPreTheta, gamma/max );
}
else {
dTheta += dPreTheta;
}*/
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if (maxChange > MaxAngleSDLS)
{
dTheta *= MaxAngleSDLS / (MaxAngleSDLS + maxChange);
//dTheta *= MaxAngleSDLS/maxChange;
}
}
void Jacobian::CalcdTClampedFromdS()
{
long len = dS.GetLength();
long j = 0;
for (long i = 0; i < len; i += 3, j++)
{
double normSq = Square(dS[i]) + Square(dS[i + 1]) + Square(dS[i + 2]);
if (normSq > Square(dSclamp[j]))
{
double factor = dSclamp[j] / sqrt(normSq);
dT1[i] = dS[i] * factor;
dT1[i + 1] = dS[i + 1] * factor;
dT1[i + 2] = dS[i + 2] * factor;
}
else
{
dT1[i] = dS[i];
dT1[i + 1] = dS[i + 1];
dT1[i + 2] = dS[i + 2];
}
}
}
double Jacobian::UpdateErrorArray(VectorR3* targets)
{
double totalError = 0.0;
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
temp = targetPos;
temp -= n->GetS();
double err = temp.Norm();
errorArray[i] = err;
totalError += err;
}
n = m_tree->GetSuccessor(n);
}
return totalError;
}
void Jacobian::UpdatedSClampValue(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
// While we are at it, also update the clamping values in dSclamp;
temp = targetPos;
temp -= n->GetS();
double normSi = sqrt(Square(dS[i]) + Square(dS[i + 1]) + Square(dS[i + 2]));
double changedDist = temp.Norm() - normSi;
if (changedDist > 0.0)
{
dSclamp[i] = BaseMaxTargetDist + changedDist;
}
else
{
dSclamp[i] = BaseMaxTargetDist;
}
}
n = m_tree->GetSuccessor(n);
}
}
void Jacobian::CompareErrors(const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2)
{
const VectorRn& e1 = j1.errorArray;
const VectorRn& e2 = j2.errorArray;
double ret1 = 0.0;
double ret2 = 0.0;
int len = e1.GetLength();
for (long i = 0; i < len; i++)
{
double v1 = e1[i];
double v2 = e2[i];
if (v1 < v2)
{
ret1 += v1 / v2;
ret2 += 1.0;
}
else if (v1 != 0.0)
{
ret1 += 1.0;
ret2 += v2 / v1;
}
else
{
ret1 += 0.0;
ret2 += 0.0;
}
}
*weightedDist1 = ret1;
*weightedDist2 = ret2;
}
void Jacobian::CountErrors(const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies)
{
const VectorRn& e1 = j1.errorArray;
const VectorRn& e2 = j2.errorArray;
int b1 = 0, b2 = 0, tie = 0;
int len = e1.GetLength();
for (long i = 0; i < len; i++)
{
double v1 = e1[i];
double v2 = e2[i];
if (v1 < v2)
{
b1++;
}
else if (v2 < v1)
{
b2++;
}
else
{
tie++;
}
}
*numBetter1 = b1;
*numBetter2 = b2;
*numTies = tie;
}
/* THIS VERSION IS NOT AS GOOD. DO NOT USE!
void Jacobian::CalcDeltaThetasSDLSrev2()
{
const MatrixRmn& J = ActiveJacobian();
// Compute Singular Value Decomposition
J.ComputeSVD( U, w, V );
// Next line for debugging only
assert(J.DebugCheckSVD(U, w , V));
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
int nRows = J.GetNumRows();
int numEndEffectors = tree->GetNumEffector(); // Equals the number of rows of J divided by three
int nCols = J.GetNumColumns();
dTheta.SetZero();
// Calculate the norms of the 3-vectors in the Jacobian
long i;
const double *jx = J.GetPtr();
double *jnx = Jnorms.GetPtr();
for ( i=nCols*numEndEffectors; i>0; i-- ) {
double accumSq = Square(*(jx++));
accumSq += Square(*(jx++));
accumSq += Square(*(jx++));
*(jnx++) = sqrt(accumSq);
}
// Loop over each singular vector
for ( i=0; i<nRows; i++ ) {
double wiInv = w[i];
if ( NearZero(wiInv,1.0e-10) ) {
continue;
}
double N = 0.0; // N is the quasi-1-norm of the i-th column of U
double alpha = 0.0; // alpha is the dot product of dS and the i-th column of U
const double *dSx = dS.GetPtr();
const double *ux = U.GetColumnPtr(i);
long j;
for ( j=numEndEffectors; j>0; j-- ) {
double tmp;
alpha += (*ux)*(*(dSx++));
tmp = Square( *(ux++) );
alpha += (*ux)*(*(dSx++));
tmp += Square(*(ux++));
alpha += (*ux)*(*(dSx++));
tmp += Square(*(ux++));
N += sqrt(tmp);
}
// P is the quasi-1-norm of the response to angles changing according to the i-th column of V
double P = 0.0;
double *vx = V.GetColumnPtr(i);
jnx = Jnorms.GetPtr();
for ( j=nCols; j>0; j-- ) {
double accum=0.0;
for ( long k=numEndEffectors; k>0; k-- ) {
accum += *(jnx++);
}
P += fabs((*(vx++)))*accum;
}
double lambda = 1.0;
if ( N<P ) {
lambda -= N/P; // Scale back maximum permissable joint angle
}
lambda *= lambda;
lambda *= DampingLambdaSDLS;
// Calculate the dTheta from pure pseudoinverse considerations
double scale = alpha*wiInv/(Square(wiInv)+Square(lambda)); // This times i-th column of V is the SDLS response
MatrixRmn::AddArrayScale(nCols, V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, scale );
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleSDLS ) {
dTheta *= MaxAngleSDLS/maxChange;
}
} */

146
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Jacobian.h Normal file
View file

@ -0,0 +1,146 @@
/*
*
* Inverse Kinematics software, with several solvers including
* Selectively Damped Least Squares Method
* Damped Least Squares Method
* Pure Pseudoinverse Method
* Jacobian Transpose Method
*
*
* Author: Samuel R. Buss, sbuss@ucsd.edu.
* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
*
*
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 "Node.h"
#include "Tree.h"
#include "MathMisc.h"
#include "LinearR3.h"
#include "VectorRn.h"
#include "MatrixRmn.h"
#ifndef _CLASS_JACOBIAN
#define _CLASS_JACOBIAN
#ifdef _DYNAMIC
const double BASEMAXDIST = 0.02;
#else
const double MAXDIST = 0.08; // optimal value for double Y shape : 0.08
#endif
const double DELTA = 0.4;
const long double LAMBDA = 2.0; // only for DLS. optimal : 0.24
const double NEARZERO = 0.0000000001;
enum UpdateMode
{
JACOB_Undefined = 0,
JACOB_JacobianTranspose = 1,
JACOB_PseudoInverse = 2,
JACOB_DLS = 3,
JACOB_SDLS = 4
};
class Jacobian
{
public:
Jacobian(Tree*);
Jacobian(bool useAngularJacobian, int nDof, int numEndEffectors);
void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
void SetJtargetActive() { Jactive = &Jtarget; }
void SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S);
void CalcDeltaThetas(MatrixRmn& AugMat); // Use this only if the Current Mode has been set.
void ZeroDeltaThetas();
void CalcDeltaThetasTranspose();
void CalcDeltaThetasPseudoinverse();
void CalcDeltaThetasDLS(MatrixRmn& AugMat);
void CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat);
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat);
void UpdateThetas();
void UpdateThetaDot();
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
const VectorRn& GetErrorArray() const { return errorArray; }
void UpdatedSClampValue(VectorR3* targets);
void SetCurrentMode(UpdateMode mode) { CurrentUpdateMode = mode; }
UpdateMode GetCurrentMode() const { return CurrentUpdateMode; }
void SetDampingDLS(double lambda)
{
DampingLambda = lambda;
DampingLambdaSq = Square(lambda);
}
void Reset();
static void CompareErrors(const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2);
static void CountErrors(const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies);
int GetNumRows() { return nRow; }
int GetNumCols() { return nCol; }
public:
Tree* m_tree; // tree associated with this Jacobian matrix
int m_nEffector; // Number of end effectors
int nJoint; // Number of joints
int nRow; // Total number of rows the real J (= 3*number of end effectors for now)
int nCol; // Total number of columns in the real J (= number of joints for now)
MatrixRmn Jend; // Jacobian matrix based on end effector positions
MatrixRmn Jtarget; // Jacobian matrix based on target positions
MatrixRmn Jnorms; // Norms of 3-vectors in active Jacobian (SDLS only)
MatrixRmn U; // J = U * Diag(w) * V^T (Singular Value Decomposition)
VectorRn w;
MatrixRmn V;
UpdateMode CurrentUpdateMode;
VectorRn dS; // delta s
VectorRn dT1; // delta t -- these are delta S values clamped to smaller magnitude
VectorRn dSclamp; // Value to clamp magnitude of dT at.
VectorRn dTheta; // delta theta
VectorRn dPreTheta; // delta theta for single eigenvalue (SDLS only)
VectorRn errorArray; // Distance of end effectors from target after updating
// Parameters for pseudoinverses
static const double PseudoInverseThresholdFactor; // Threshold for treating eigenvalue as zero (fraction of largest eigenvalue)
// Parameters for damped least squares
static const double DefaultDampingLambda;
double DampingLambda;
double DampingLambdaSq;
//double DampingLambdaSDLS;
// Cap on max. value of changes in angles in single update step
static const double MaxAngleJtranspose;
static const double MaxAnglePseudoinverse;
static const double MaxAngleDLS;
static const double MaxAngleSDLS;
MatrixRmn* Jactive;
void CalcdTClampedFromdS();
static const double BaseMaxTargetDist;
};
#endif

12
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LICENSE.txt Normal file
View file

@ -0,0 +1,12 @@
* Author: Samuel R. Buss, sbuss@ucsd.edu.
* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
*
*
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.

89
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LinearR2.cpp Normal file
View file

@ -0,0 +1,89 @@
/*
*
* 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 "LinearR2.h"
#include <assert.h>
// ******************************************************
// * VectorR2 class - math library functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
const VectorR2 VectorR2::Zero(0.0, 0.0);
const VectorR2 VectorR2::UnitX(1.0, 0.0);
const VectorR2 VectorR2::UnitY(0.0, 1.0);
const VectorR2 VectorR2::NegUnitX(-1.0, 0.0);
const VectorR2 VectorR2::NegUnitY(0.0, -1.0);
const Matrix2x2 Matrix2x2::Identity(1.0, 0.0, 0.0, 1.0);
// ******************************************************
// * Matrix2x2 class - math library functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
// ******************************************************
// * LinearMapR2 class - math library functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
LinearMapR2 LinearMapR2::Inverse() const // Returns inverse
{
double detInv = 1.0 / (m11 * m22 - m12 * m21);
return (LinearMapR2(m22 * detInv, -m21 * detInv, -m12 * detInv, m11 * detInv));
}
LinearMapR2& LinearMapR2::Invert() // Converts into inverse.
{
double detInv = 1.0 / (m11 * m22 - m12 * m21);
double temp;
temp = m11 * detInv;
m11 = m22 * detInv;
m22 = temp;
m12 = -m12 * detInv;
m21 = -m22 * detInv;
return (*this);
}
VectorR2 LinearMapR2::Solve(const VectorR2& u) const // Returns solution
{
// Just uses Inverse() for now.
return (Inverse() * u);
}
// ******************************************************
// * RotationMapR2 class - math library functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
// ***************************************************************
// * 2-space vector and matrix utilities *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
// ***************************************************************
// Stream Output Routines *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
ostream& operator<<(ostream& os, const VectorR2& u)
{
return (os << "<" << u.x << "," << u.y << ">");
}

1025
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LinearR2.h Normal file
View file

File diff suppressed because it is too large Load diff

831
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LinearR3.cpp Normal file
View file

@ -0,0 +1,831 @@
/*
*
* 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);
}

2127
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LinearR3.h Normal file
View file

File diff suppressed because it is too large Load diff

467
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LinearR4.cpp Normal file
View file

@ -0,0 +1,467 @@
/*
*
* 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 << ">");
}

1171
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LinearR4.h Normal file
View file

File diff suppressed because it is too large Load diff

447
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/MathMisc.h Normal file
View file

@ -0,0 +1,447 @@
/*
*
* 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.
*
*
*/
#ifndef MATH_MISC_H
#define MATH_MISC_H
#include <math.h>
//
// Commonly used constants
//
const double PI = 3.1415926535897932384626433832795028841972;
const double PI2 = 2.0 * PI;
const double PI4 = 4.0 * PI;
const double PISq = PI * PI;
const double PIhalves = 0.5 * PI;
const double PIthirds = PI / 3.0;
const double PItwothirds = PI2 / 3.0;
const double PIfourths = 0.25 * PI;
const double PIsixths = PI / 6.0;
const double PIsixthsSq = PIsixths * PIsixths;
const double PItwelfths = PI / 12.0;
const double PItwelfthsSq = PItwelfths * PItwelfths;
const double PIinv = 1.0 / PI;
const double PI2inv = 0.5 / PI;
const double PIhalfinv = 2.0 / PI;
const double RadiansToDegrees = 180.0 / PI;
const double DegreesToRadians = PI / 180;
const double OneThird = 1.0 / 3.0;
const double TwoThirds = 2.0 / 3.0;
const double OneSixth = 1.0 / 6.0;
const double OneEighth = 1.0 / 8.0;
const double OneTwelfth = 1.0 / 12.0;
const double Root2 = sqrt(2.0);
const double Root3 = sqrt(3.0);
const double Root2Inv = 1.0 / Root2; // sqrt(2)/2
const double HalfRoot3 = sqrtf(3) / 2.0;
const double LnTwo = log(2.0);
const double LnTwoInv = 1.0 / log(2.0);
// Special purpose constants
const double OnePlusEpsilon15 = 1.0 + 1.0e-15;
const double OneMinusEpsilon15 = 1.0 - 1.0e-15;
inline double ZeroValue(const double& x)
{
return 0.0;
}
//
// Comparisons
//
template <class T>
inline T Min(T x, T y)
{
return (x < y ? x : y);
}
template <class T>
inline T Max(T x, T y)
{
return (y < x ? x : y);
}
template <class T>
inline T ClampRange(T x, T min, T max)
{
if (x < min)
{
return min;
}
if (x > max)
{
return max;
}
return x;
}
template <class T>
inline bool ClampRange(T* x, T min, T max)
{
if ((*x) < min)
{
(*x) = min;
return false;
}
else if ((*x) > max)
{
(*x) = max;
return false;
}
else
{
return true;
}
}
template <class T>
inline bool ClampMin(T* x, T min)
{
if ((*x) < min)
{
(*x) = min;
return false;
}
return true;
}
template <class T>
inline bool ClampMax(T* x, T max)
{
if ((*x) > max)
{
(*x) = max;
return false;
}
return true;
}
template <class T>
inline T& UpdateMin(const T& x, T& y)
{
if (x < y)
{
y = x;
}
return y;
}
template <class T>
inline T& UpdateMax(const T& x, T& y)
{
if (x > y)
{
y = x;
}
return y;
}
template <class T>
inline bool SameSignNonzero(T x, T y)
{
if (x < 0)
{
return (y < 0);
}
else if (0 < x)
{
return (0 < y);
}
else
{
return false;
}
}
inline double Mag(double x)
{
return fabs(x);
}
inline double Dist(double x, double y)
{
return fabs(x - y);
}
template <class T>
inline bool NearEqual(T a, T b, double tolerance)
{
a -= b;
return (Mag(a) <= tolerance);
}
inline bool EqualZeroFuzzy(double x)
{
return (fabs(x) <= 1.0e-14);
}
inline bool NearZero(double x, double tolerance)
{
return (fabs(x) <= tolerance);
}
inline bool LessOrEqualFuzzy(double x, double y)
{
if (x <= y)
{
return true;
}
if (y > 0.0)
{
if (x > 0.0)
{
return (x * OneMinusEpsilon15 < y * OnePlusEpsilon15);
}
else
{
return (y < 1.0e-15); // x==0 in this case
}
}
else if (y < 0.0)
{
if (x < 0.0)
{
return (x * OnePlusEpsilon15 < y * OneMinusEpsilon15);
}
else
{
return (y > -1.0e-15); // x==0 in this case
}
}
else
{
return (-1.0e-15 < x && x < 1.0e-15);
}
}
inline bool GreaterOrEqualFuzzy(double x, double y)
{
return LessOrEqualFuzzy(y, x);
}
inline bool UpdateMaxAbs(double* maxabs, double updateval)
{
if (updateval > *maxabs)
{
*maxabs = updateval;
return true;
}
else if (-updateval > *maxabs)
{
*maxabs = -updateval;
return true;
}
else
{
return false;
}
}
// **********************************************************
// Combinations and averages. *
// **********************************************************
template <class T>
void averageOf(const T& a, const T& b, T& c)
{
c = a;
c += b;
c *= 0.5;
}
template <class T>
void Lerp(const T& a, const T& b, double alpha, T& c)
{
double beta = 1.0 - alpha;
if (beta > alpha)
{
c = b;
c *= alpha / beta;
c += a;
c *= beta;
}
else
{
c = a;
c *= beta / alpha;
c += b;
c *= alpha;
}
}
template <class T>
T Lerp(const T& a, const T& b, double alpha)
{
T ret;
Lerp(a, b, alpha, ret);
return ret;
}
// **********************************************************
// Trigonometry *
// **********************************************************
// TimesCot(x) returns x*cot(x)
inline double TimesCot(double x)
{
if (-1.0e-5 < x && x < 1.0e-5)
{
return 1.0 + x * OneThird;
}
else
{
return (x * cos(x) / sin(x));
}
}
// SineOver(x) returns sin(x)/x.
inline double SineOver(double x)
{
if (-1.0e-5 < x && x < 1.0e-5)
{
return 1.0 - x * x * OneSixth;
}
else
{
return sin(x) / x;
}
}
// OverSine(x) returns x/sin(x).
inline double OverSine(double x)
{
if (-1.0e-5 < x && x < 1.0e-5)
{
return 1.0 + x * x * OneSixth;
}
else
{
return x / sin(x);
}
}
inline double SafeAsin(double x)
{
if (x <= -1.0)
{
return -PIhalves;
}
else if (x >= 1.0)
{
return PIhalves;
}
else
{
return asin(x);
}
}
inline double SafeAcos(double x)
{
if (x <= -1.0)
{
return PI;
}
else if (x >= 1.0)
{
return 0.0;
}
else
{
return acos(x);
}
}
// **********************************************************************
// Roots and powers *
// **********************************************************************
// Square(x) returns x*x, of course!
template <class T>
inline T Square(T x)
{
return (x * x);
}
// Cube(x) returns x*x*x, of course!
template <class T>
inline T Cube(T x)
{
return (x * x * x);
}
// SafeSqrt(x) = returns sqrt(max(x, 0.0));
inline double SafeSqrt(double x)
{
if (x <= 0.0)
{
return 0.0;
}
else
{
return sqrt(x);
}
}
// SignedSqrt(a, s) returns (sign(s)*sqrt(a)).
inline double SignedSqrt(double a, double sgn)
{
if (sgn == 0.0)
{
return 0.0;
}
else
{
return (sgn > 0.0 ? sqrt(a) : -sqrt(a));
}
}
// Template version of Sign function
template <class T>
inline int Sign(T x)
{
if (x < 0)
{
return -1;
}
else if (x == 0)
{
return 0;
}
else
{
return 1;
}
}
#endif // #ifndef MATH_MISC_H

1111
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp Normal file
View file

File diff suppressed because it is too large Load diff

417
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/MatrixRmn.h Normal file
View file

@ -0,0 +1,417 @@
/*
*
* 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.
*
*
*/
//
// MatrixRmn: Matrix over reals (Variable dimensional vector)
//
// Not very sophisticated yet. Needs more functionality
// To do: better handling of resizing.
//
#ifndef MATRIX_RMN_H
#define MATRIX_RMN_H
#include <math.h>
#include <assert.h>
#include "LinearR3.h"
#include "VectorRn.h"
class MatrixRmn
{
public:
MatrixRmn(); // Null constructor
MatrixRmn(long numRows, long numCols); // Constructor with length
~MatrixRmn(); // Destructor
void SetSize(long numRows, long numCols);
long GetNumRows() const { return NumRows; }
long GetNumColumns() const { return NumCols; }
void SetZero();
// Return entry in row i and column j.
double Get(long i, long j) const;
void GetTriple(long i, long j, VectorR3* retValue) const;
// Use GetPtr to get pointer into the array (efficient)
// Is friendly in that anyone can change the array contents (be careful!)
// The entries are in column order!!!
// Use this with care. You may call GetRowStride and GetColStride to navigate
// within the matrix. I do not expect these values to ever change.
const double* GetPtr() const;
double* GetPtr();
const double* GetPtr(long i, long j) const;
double* GetPtr(long i, long j);
const double* GetColumnPtr(long j) const;
double* GetColumnPtr(long j);
const double* GetRowPtr(long i) const;
double* GetRowPtr(long i);
long GetRowStride() const { return NumRows; } // Step size (stride) along a row
long GetColStride() const { return 1; } // Step size (stide) along a column
void Set(long i, long j, double val);
void SetTriple(long i, long c, const VectorR3& u);
void SetIdentity();
void SetDiagonalEntries(double d);
void SetDiagonalEntries(const VectorRn& d);
void SetSuperDiagonalEntries(double d);
void SetSuperDiagonalEntries(const VectorRn& d);
void SetSubDiagonalEntries(double d);
void SetSubDiagonalEntries(const VectorRn& d);
void SetColumn(long i, const VectorRn& d);
void SetRow(long i, const VectorRn& d);
void SetSequence(const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol);
// Loads matrix in as a sub-matrix. (i,j) is the base point. Defaults to (0,0).
// The "Tranpose" versions load the transpose of A.
void LoadAsSubmatrix(const MatrixRmn& A);
void LoadAsSubmatrix(long i, long j, const MatrixRmn& A);
void LoadAsSubmatrixTranspose(const MatrixRmn& A);
void LoadAsSubmatrixTranspose(long i, long j, const MatrixRmn& A);
// Norms
double FrobeniusNormSq() const;
double FrobeniusNorm() const;
// Operations on VectorRn's
void Multiply(const VectorRn& v, VectorRn& result) const; // result = (this)*(v)
void MultiplyTranspose(const VectorRn& v, VectorRn& result) const; // Equivalent to mult by row vector on left
double DotProductColumn(const VectorRn& v, long colNum) const; // Returns dot product of v with i-th column
// Operations on MatrixRmn's
MatrixRmn& operator*=(double);
MatrixRmn& operator/=(double d)
{
assert(d != 0.0);
*this *= (1.0 / d);
return *this;
}
MatrixRmn& AddScaled(const MatrixRmn& B, double factor);
MatrixRmn& operator+=(const MatrixRmn& B);
MatrixRmn& operator-=(const MatrixRmn& B);
static MatrixRmn& Multiply(const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst); // Sets dst = A*B.
static MatrixRmn& MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst); // Sets dst = A*(B-tranpose).
static MatrixRmn& TransposeMultiply(const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst); // Sets dst = (A-transpose)*B.
// Miscellaneous operation
MatrixRmn& AddToDiagonal(double d); // Adds d to each diagonal
MatrixRmn& AddToDiagonal(const VectorRn& dVec);
// Solving systems of linear equations
void Solve(const VectorRn& b, VectorRn* x, MatrixRmn& AugMat) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible.
// Row Echelon Form and Reduced Row Echelon Form routines
// Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables.
void ConvertToRefNoFree(); // Converts the matrix in place to row echelon form -- assumption is no free variables will be found
void ConvertToRef(int numVars); // Converts the matrix in place to row echelon form -- numVars is number of columns to work with.
void ConvertToRef(int numVars, double eps); // Same, but eps is the measure of closeness to zero
// Givens transformation
static void CalcGivensValues(double a, double b, double* c, double* s);
void PostApplyGivens(double c, double s, long idx); // Applies Givens transform to columns idx and idx+1.
void PostApplyGivens(double c, double s, long idx1, long idx2); // Applies Givens transform to columns idx1 and idx2.
// Singular value decomposition
void ComputeSVD(MatrixRmn& U, VectorRn& w, MatrixRmn& V) const;
// Good for debugging SVD computations (I recommend this be used for any new application to check for bugs/instability).
bool DebugCheckSVD(const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V) const;
// Compute inverse of a matrix, the result is written in R
void ComputeInverse(MatrixRmn& R) const;
// Debug matrix inverse computation
bool DebugCheckInverse(const MatrixRmn& MInv) const;
// Some useful routines for experts who understand the inner workings of these classes.
inline static double DotArray(long length, const double* ptrA, long strideA, const double* ptrB, long strideB);
inline static void CopyArrayScale(long length, const double* from, long fromStride, double* to, long toStride, double scale);
inline static void AddArrayScale(long length, const double* from, long fromStride, double* to, long toStride, double scale);
private:
long NumRows; // Number of rows
long NumCols; // Number of columns
double* x; // Array of vector entries - stored in column order
long AllocSize; // Allocated size of the x array
// Internal helper routines for SVD calculations
static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag);
void ConvertBidiagToDiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag) const;
static void SvdHouseholder(double* basePt,
long colLength, long numCols, long colStride, long rowStride,
double* retFirstEntry);
void ExpandHouseholders(long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride);
static bool UpdateBidiagIndices(long* firstDiagIdx, long* lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps);
static void ApplyGivensCBTD(double cosine, double sine, double* a, double* b, double* c, double* d);
static void ApplyGivensCBTD(double cosine, double sine, double* a, double* b, double* c,
double d, double* e, double* f);
static void ClearRowWithDiagonalZero(long firstBidiagIdx, long lastBidiagIdx,
MatrixRmn& U, double* wPtr, double* sdPtr, double eps);
static void ClearColumnWithDiagonalZero(long endIdx, MatrixRmn& V, double* wPtr, double* sdPtr, double eps);
bool DebugCalcBidiagCheck(const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V) const;
};
inline MatrixRmn::MatrixRmn()
{
NumRows = 0;
NumCols = 0;
x = 0;
AllocSize = 0;
}
inline MatrixRmn::MatrixRmn(long numRows, long numCols)
{
NumRows = 0;
NumCols = 0;
x = 0;
AllocSize = 0;
SetSize(numRows, numCols);
}
inline MatrixRmn::~MatrixRmn()
{
delete[] x;
}
// Resize.
// If the array space is decreased, the information about the allocated length is lost.
inline void MatrixRmn::SetSize(long numRows, long numCols)
{
assert(numRows > 0 && numCols > 0);
long newLength = numRows * numCols;
if (newLength > AllocSize)
{
delete[] x;
AllocSize = Max(newLength, AllocSize << 1);
x = new double[AllocSize];
}
NumRows = numRows;
NumCols = numCols;
}
// Zero out the entire vector
inline void MatrixRmn::SetZero()
{
double* target = x;
for (long i = NumRows * NumCols; i > 0; i--)
{
*(target++) = 0.0;
}
}
// Return entry in row i and column j.
inline double MatrixRmn::Get(long i, long j) const
{
assert(i < NumRows && j < NumCols);
return *(x + j * NumRows + i);
}
// Return a VectorR3 out of a column. Starts at row 3*i, in column j.
inline void MatrixRmn::GetTriple(long i, long j, VectorR3* retValue) const
{
long ii = 3 * i;
assert(0 <= i && ii + 2 < NumRows && 0 <= j && j < NumCols);
retValue->Load(x + j * NumRows + ii);
}
// Get a pointer to the (0,0) entry.
// The entries are in column order.
// This version gives read-only pointer
inline const double* MatrixRmn::GetPtr() const
{
return x;
}
// Get a pointer to the (0,0) entry.
// The entries are in column order.
inline double* MatrixRmn::GetPtr()
{
return x;
}
// Get a pointer to the (i,j) entry.
// The entries are in column order.
// This version gives read-only pointer
inline const double* MatrixRmn::GetPtr(long i, long j) const
{
assert(0 <= i && i < NumRows && 0 <= j && j < NumCols);
return (x + j * NumRows + i);
}
// Get a pointer to the (i,j) entry.
// The entries are in column order.
// This version gives pointer to writable data
inline double* MatrixRmn::GetPtr(long i, long j)
{
assert(i < NumRows && j < NumCols);
return (x + j * NumRows + i);
}
// Get a pointer to the j-th column.
// The entries are in column order.
// This version gives read-only pointer
inline const double* MatrixRmn::GetColumnPtr(long j) const
{
assert(0 <= j && j < NumCols);
return (x + j * NumRows);
}
// Get a pointer to the j-th column.
// This version gives pointer to writable data
inline double* MatrixRmn::GetColumnPtr(long j)
{
assert(0 <= j && j < NumCols);
return (x + j * NumRows);
}
/// Get a pointer to the i-th row
// The entries are in column order.
// This version gives read-only pointer
inline const double* MatrixRmn::GetRowPtr(long i) const
{
assert(0 <= i && i < NumRows);
return (x + i);
}
// Get a pointer to the i-th row
// This version gives pointer to writable data
inline double* MatrixRmn::GetRowPtr(long i)
{
assert(0 <= i && i < NumRows);
return (x + i);
}
// Set the (i,j) entry of the matrix
inline void MatrixRmn::Set(long i, long j, double val)
{
assert(i < NumRows && j < NumCols);
*(x + j * NumRows + i) = val;
}
// Set the i-th triple in the j-th column to u's three values
inline void MatrixRmn::SetTriple(long i, long j, const VectorR3& u)
{
long ii = 3 * i;
assert(0 <= i && ii + 2 < NumRows && 0 <= j && j < NumCols);
u.Dump(x + j * NumRows + ii);
}
// Set to be equal to the identity matrix
inline void MatrixRmn::SetIdentity()
{
assert(NumRows == NumCols);
SetZero();
SetDiagonalEntries(1.0);
}
inline MatrixRmn& MatrixRmn::operator*=(double mult)
{
double* aPtr = x;
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) *= mult;
}
return (*this);
}
inline MatrixRmn& MatrixRmn::AddScaled(const MatrixRmn& B, double factor)
{
assert(NumRows == B.NumRows && NumCols == B.NumCols);
double* aPtr = x;
double* bPtr = B.x;
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) += (*(bPtr++)) * factor;
}
return (*this);
}
inline MatrixRmn& MatrixRmn::operator+=(const MatrixRmn& B)
{
assert(NumRows == B.NumRows && NumCols == B.NumCols);
double* aPtr = x;
double* bPtr = B.x;
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) += *(bPtr++);
}
return (*this);
}
inline MatrixRmn& MatrixRmn::operator-=(const MatrixRmn& B)
{
assert(NumRows == B.NumRows && NumCols == B.NumCols);
double* aPtr = x;
double* bPtr = B.x;
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) -= *(bPtr++);
}
return (*this);
}
inline double MatrixRmn::FrobeniusNormSq() const
{
double* aPtr = x;
double result = 0.0;
for (long i = NumRows * NumCols; i > 0; i--)
{
result += Square(*(aPtr++));
}
return result;
}
// Helper routine to calculate dot product
inline double MatrixRmn::DotArray(long length, const double* ptrA, long strideA, const double* ptrB, long strideB)
{
double result = 0.0;
for (; length > 0; length--)
{
result += (*ptrA) * (*ptrB);
ptrA += strideA;
ptrB += strideB;
}
return result;
}
// Helper routine: copies and scales an array (src and dest may be equal, or overlap)
inline void MatrixRmn::CopyArrayScale(long length, const double* from, long fromStride, double* to, long toStride, double scale)
{
for (; length > 0; length--)
{
*to = (*from) * scale;
from += fromStride;
to += toStride;
}
}
// Helper routine: adds a scaled array
// fromArray = toArray*scale.
inline void MatrixRmn::AddArrayScale(long length, const double* from, long fromStride, double* to, long toStride, double scale)
{
for (; length > 0; length--)
{
*to += (*from) * scale;
from += fromStride;
to += toStride;
}
}
#endif //MATRIX_RMN_H

140
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Misc.cpp Normal file
View file

@ -0,0 +1,140 @@
/*
*
* 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 <math.h>
#include <cmath>
#include "LinearR3.h"
#if 0
/****************************************************************
Axes
*****************************************************************/
static float xx[] = {
0., 1., 0., 1.
};
static float xy[] = {
-.5, .5, .5, -.5
};
static int xorder[] = {
1, 2, -3, 4
};
static float yx[] = {
0., 0., -.5, .5
};
static float yy[] = {
0.f, .6f, 1.f, 1.f
};
static int yorder[] = {
1, 2, 3, -2, 4
};
static float zx[] = {
1., 0., 1., 0., .25, .75
};
static float zy[] = {
.5, .5, -.5, -.5, 0., 0.
};
static int zorder[] = {
1, 2, 3, 4, -5, 6
};
#endif
#define LENFRAC 0.10
#define BASEFRAC 1.10
/****************************************************************
Arrow
*****************************************************************/
/* size of wings as fraction of length: */
#define WINGS 0.10
/* axes: */
#define X 1
#define Y 2
#define Z 3
/* x, y, z, axes: */
//static float axx[3] = { 1., 0., 0. };
//static float ayy[3] = { 0., 1., 0. };
//static float azz[3] = { 0., 0., 1. };
/* function declarations: */
void cross(float[3], float[3], float[3]);
float dot(float[3], float[3]);
float unit(float[3], float[3]);
float dot(float v1[3], float v2[3])
{
return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
}
void cross(float v1[3], float v2[3], float vout[3])
{
float tmp[3];
tmp[0] = v1[1] * v2[2] - v2[1] * v1[2];
tmp[1] = v2[0] * v1[2] - v1[0] * v2[2];
tmp[2] = v1[0] * v2[1] - v2[0] * v1[1];
vout[0] = tmp[0];
vout[1] = tmp[1];
vout[2] = tmp[2];
}
float unit(float vin[3], float vout[3])
{
float dist, f;
dist = vin[0] * vin[0] + vin[1] * vin[1] + vin[2] * vin[2];
if (dist > 0.0)
{
dist = std::sqrt(dist);
f = 1. / dist;
vout[0] = f * vin[0];
vout[1] = f * vin[1];
vout[2] = f * vin[2];
}
else
{
vout[0] = vin[0];
vout[1] = vin[1];
vout[2] = vin[2];
}
return (dist);
}

87
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Node.cpp Normal file
View file

@ -0,0 +1,87 @@
/*
*
* 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 <math.h>
#include "LinearR3.h"
#include "MathMisc.h"
#include "Node.h"
extern int RotAxesOn;
Node::Node(const VectorR3& attach, const VectorR3& v, double size, Purpose purpose, double minTheta, double maxTheta, double restAngle)
{
Node::freezed = false;
Node::size = size;
Node::purpose = purpose;
seqNumJoint = -1;
seqNumEffector = -1;
Node::attach = attach; // Global attachment point when joints are at zero angle
r.Set(0.0, 0.0, 0.0); // r will be updated when this node is inserted into tree
Node::v = v; // Rotation axis when joints at zero angles
theta = 0.0;
Node::minTheta = minTheta;
Node::maxTheta = maxTheta;
Node::restAngle = restAngle;
left = right = realparent = 0;
}
// Compute the global position of a single node
void Node::ComputeS(void)
{
Node* y = this->realparent;
Node* w = this;
s = r; // Initialize to local (relative) position
while (y)
{
s.Rotate(y->theta, y->v);
y = y->realparent;
w = w->realparent;
s += w->r;
}
}
// Compute the global rotation axis of a single node
void Node::ComputeW(void)
{
Node* y = this->realparent;
w = v; // Initialize to local rotation axis
while (y)
{
w.Rotate(y->theta, y->v);
y = y->realparent;
}
}
void Node::PrintNode()
{
cerr << "Attach : (" << attach << ")\n";
cerr << "r : (" << r << ")\n";
cerr << "s : (" << s << ")\n";
cerr << "w : (" << w << ")\n";
cerr << "realparent : " << realparent->seqNumJoint << "\n";
}
void Node::InitNode()
{
theta = 0.0;
}

109
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Node.h Normal file
View file

@ -0,0 +1,109 @@
/*
*
* 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.
*
*
*/
#ifndef _CLASS_NODE
#define _CLASS_NODE
#include "LinearR3.h"
enum Purpose
{
JOINT,
EFFECTOR
};
class VectorR3;
class Node
{
friend class Tree;
public:
Node(const VectorR3&, const VectorR3&, double, Purpose, double minTheta = -PI, double maxTheta = PI, double restAngle = 0.);
void PrintNode();
void InitNode();
const VectorR3& GetAttach() const { return attach; }
double GetTheta() const { return theta; }
double AddToTheta(double& delta)
{
//double orgTheta = theta;
theta += delta;
#if 0
if (theta < minTheta)
theta = minTheta;
if (theta > maxTheta)
theta = maxTheta;
double actualDelta = theta - orgTheta;
delta = actualDelta;
#endif
return theta;
}
double UpdateTheta(double& delta)
{
theta = delta;
return theta;
}
const VectorR3& GetS() const { return s; }
const VectorR3& GetW() const { return w; }
double GetMinTheta() const { return minTheta; }
double GetMaxTheta() const { return maxTheta; }
double GetRestAngle() const { return restAngle; };
void SetTheta(double newTheta) { theta = newTheta; }
void ComputeS(void);
void ComputeW(void);
bool IsEffector() const { return purpose == EFFECTOR; }
bool IsJoint() const { return purpose == JOINT; }
int GetEffectorNum() const { return seqNumEffector; }
int GetJointNum() const { return seqNumJoint; }
bool IsFrozen() const { return freezed; }
void Freeze() { freezed = true; }
void UnFreeze() { freezed = false; }
//private:
bool freezed; // Is this node frozen?
int seqNumJoint; // sequence number if this node is a joint
int seqNumEffector; // sequence number if this node is an effector
double size; // size
Purpose purpose; // joint / effector / both
VectorR3 attach; // attachment point
VectorR3 r; // relative position vector
VectorR3 v; // rotation axis
double theta; // joint angle (radian)
double minTheta; // lower limit of joint angle
double maxTheta; // upper limit of joint angle
double restAngle; // rest position angle
VectorR3 s; // GLobal Position
VectorR3 w; // Global rotation axis
Node* left; // left child
Node* right; // right sibling
Node* realparent; // pointer to real parent
};
#endif

293
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Spherical.h Normal file
View file

@ -0,0 +1,293 @@
/*
*
* 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.
*
*
*/
//
// Spherical Operations Classes
//
//
// B. SphericalInterpolator
//
// OrientationR3
//
// A. Quaternion
//
// B. RotationMapR3 // Elsewhere
//
// C. EulerAnglesR3 // TO DO
//
//
// Functions for spherical operations
// A. Many routines for rotation and averaging on a sphere
//
#ifndef SPHERICAL_H
#define SPHERICAL_H
#include "LinearR3.h"
#include "LinearR4.h"
#include "MathMisc.h"
class SphericalInterpolator; // Spherical linear interpolation of vectors
class SphericalBSpInterpolator; // Spherical Bspline interpolation of vector
class Quaternion; // Quaternion (x,y,z,w) values.
class EulerAnglesR3; // Euler Angles
// *****************************************************
// SphericalInterpolator class *
// - Does linear interpolation (slerp-ing) *
// * * * * * * * * * * * * * * * * * * * * * * * * * * *
class SphericalInterpolator
{
private:
VectorR3 startDir, endDir; // Unit vectors (starting and ending)
double startLen, endLen; // Magnitudes of the vectors
double rotRate; // Angle between start and end vectors
public:
SphericalInterpolator(const VectorR3& u, const VectorR3& v);
VectorR3 InterValue(double frac) const;
};
// ***************************************************************
// * Quaternion class - prototypes *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
class Quaternion
{
public:
double x, y, z, w;
public:
Quaternion() : x(0.0), y(0.0), z(0.0), w(1.0){};
Quaternion(double, double, double, double);
inline Quaternion& Set(double xx, double yy, double zz, double ww);
inline Quaternion& Set(const VectorR4&);
Quaternion& Set(const EulerAnglesR3&);
Quaternion& Set(const RotationMapR3&);
Quaternion& SetRotate(const VectorR3&);
Quaternion& SetIdentity(); // Set to the identity map
Quaternion Inverse() const; // Return the Inverse
Quaternion& Invert(); // Invert this quaternion
double Angle(); // Angle of rotation
double Norm(); // Norm of x,y,z component
Quaternion& operator*=(const Quaternion&);
};
Quaternion operator*(const Quaternion&, const Quaternion&);
inline Quaternion ToQuat(const VectorR4& v)
{
return Quaternion(v.x, v.y, v.z, v.w);
}
inline double Quaternion::Norm()
{
return sqrt(x * x + y * y + z * z);
}
inline double Quaternion::Angle()
{
double halfAngle = asin(Norm());
return halfAngle + halfAngle;
}
// ****************************************************************
// Solid Geometry Routines *
// ****************************************************************
// Compute the angle formed by two geodesics on the unit sphere.
// Three unit vectors u,v,w specify the geodesics u-v and v-w which
// meet at vertex uv. The angle from v-w to v-u is returned. This
// is always in the range [0, 2PI).
double SphereAngle(const VectorR3& u, const VectorR3& v, const VectorR3& w);
// Compute the area of a triangle on the unit sphere. Three unit vectors
// specify the corners of the triangle in COUNTERCLOCKWISE order.
inline double SphericalTriangleArea(
const VectorR3& u, const VectorR3& v, const VectorR3& w)
{
double AngleA = SphereAngle(u, v, w);
double AngleB = SphereAngle(v, w, u);
double AngleC = SphereAngle(w, u, v);
return (AngleA + AngleB + AngleC - PI);
}
// ****************************************************************
// Spherical Mean routines *
// ****************************************************************
// Weighted sum of vectors
VectorR3 WeightedSum(long Num, const VectorR3 vv[], const double weights[]);
VectorR4 WeightedSum(long Num, const VectorR4 vv[], const double weights[]);
// Weighted average of vectors on the sphere.
// Sum of weights should equal one (but no checking is done)
VectorR3 ComputeMeanSphere(long Num, const VectorR3 vv[], const double weights[],
double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13);
VectorR3 ComputeMeanSphere(long Num, const VectorR3 vv[], const double weights[],
const VectorR3& InitialVec,
double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13);
VectorR4 ComputeMeanSphere(long Num, const VectorR4 vv[], const double weights[],
double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13);
Quaternion ComputeMeanQuat(long Num, const Quaternion qq[], const double weights[],
double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13);
// Next functions mostly for internal use.
// It takes an initial estimate InitialVec (and a flag for
// indicating quaternions).
VectorR4 ComputeMeanSphere(long Num, const VectorR4 vv[], const double weights[],
const VectorR4& InitialVec, int QuatFlag = 0,
double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13);
const int SPHERICAL_NOTQUAT = 0;
const int SPHERICAL_QUAT = 1;
// Slow version, mostly for testing
VectorR3 ComputeMeanSphereSlow(long Num, const VectorR3 vv[], const double weights[],
double tolerance = 1.0e-16, double bkuptolerance = 5.0e-16);
VectorR4 ComputeMeanSphereSlow(long Num, const VectorR4 vv[], const double weights[],
double tolerance = 1.0e-16, double bkuptolerance = 5.0e-16);
VectorR3 ComputeMeanSphereOld(long Num, const VectorR3 vv[], const double weights[],
double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13);
VectorR4 ComputeMeanSphereOld(long Num, const VectorR4 vv[], const double weights[],
const VectorR4& InitialVec, int QuatFlag,
double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13);
// Solves a system of spherical-mean equalities, where the system is
// given as a tridiagonal matrix.
void SolveTriDiagSphere(int numPoints,
const double* tridiagvalues, const VectorR3* c,
VectorR3* p,
double accuracy = 1.0e-15, double bkupaccuracy = 1.0e-13);
void SolveTriDiagSphere(int numPoints,
const double* tridiagvalues, const VectorR4* c,
VectorR4* p,
double accuracy = 1.0e-15, double bkupaccuracy = 1.0e-13);
// The "Slow" version uses a simpler but slower iteration with a linear rate of
// convergence. The base version uses a Newton iteration with a quadratic
// rate of convergence.
void SolveTriDiagSphereSlow(int numPoints,
const double* tridiagvalues, const VectorR3* c,
VectorR3* p,
double accuracy = 1.0e-15, double bkupaccuracy = 5.0e-15);
void SolveTriDiagSphereSlow(int numPoints,
const double* tridiagvalues, const VectorR4* c,
VectorR4* p,
double accuracy = 1.0e-15, double bkupaccuracy = 5.0e-15);
// The "Unstable" version probably shouldn't be used except for very short sequences
// of knots. Mostly it's used for testing purposes now.
void SolveTriDiagSphereUnstable(int numPoints,
const double* tridiagvalues, const VectorR3* c,
VectorR3* p,
double accuracy = 1.0e-15, double bkupaccuracy = 1.0e-13);
void SolveTriDiagSphereHelperUnstable(int numPoints,
const double* tridiagvalues, const VectorR3* c,
const VectorR3& p0value,
VectorR3* p,
double accuracy = 1.0e-15, double bkupaccuracy = 1.0e-13);
// ***************************************************************
// * Quaternion class - inlined member functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
inline VectorR4::VectorR4(const Quaternion& q)
: x(q.x), y(q.y), z(q.z), w(q.w)
{
}
inline VectorR4& VectorR4::Set(const Quaternion& q)
{
x = q.x;
y = q.y;
z = q.z;
w = q.w;
return *this;
}
inline Quaternion::Quaternion(double xx, double yy, double zz, double ww)
: x(xx), y(yy), z(zz), w(ww)
{
}
inline Quaternion& Quaternion::Set(double xx, double yy, double zz, double ww)
{
x = xx;
y = yy;
z = zz;
w = ww;
return *this;
}
inline Quaternion& Quaternion::Set(const VectorR4& u)
{
x = u.x;
y = u.y;
z = u.z;
w = u.w;
return *this;
}
inline Quaternion& Quaternion::SetIdentity()
{
x = y = z = 0.0;
w = 1.0;
return *this;
}
inline Quaternion operator*(const Quaternion& q1, const Quaternion& q2)
{
Quaternion q(q1);
q *= q2;
return q;
}
inline Quaternion& Quaternion::operator*=(const Quaternion& q)
{
double wnew = w * q.w - (x * q.x + y * q.y + z * q.z);
double xnew = w * q.x + q.w * x + (y * q.z - z * q.y);
double ynew = w * q.y + q.w * y + (z * q.x - x * q.z);
z = w * q.z + q.w * z + (x * q.y - y * q.x);
w = wnew;
x = xnew;
y = ynew;
return *this;
}
inline Quaternion Quaternion::Inverse() const // Return the Inverse
{
return (Quaternion(x, y, z, -w));
}
inline Quaternion& Quaternion::Invert() // Invert this quaternion
{
w = -w;
return *this;
}
#endif // SPHERICAL_H

231
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Tree.cpp Normal file
View file

@ -0,0 +1,231 @@
/*
*
* Inverse Kinematics software, with several solvers including
* Selectively Damped Least Squares Method
* Damped Least Squares Method
* Pure Pseudoinverse Method
* Jacobian Transpose Method
*
*
* Author: Samuel R. Buss, sbuss@ucsd.edu.
* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
*
*
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.
*
*
*/
//
// VectorRn: Vector over Rn (Variable length vector)
//
#include <iostream>
using namespace std;
#include "LinearR3.h"
#include "Tree.h"
#include "Node.h"
Tree::Tree()
{
root = 0;
nNode = nEffector = nJoint = 0;
}
void Tree::SetSeqNum(Node* node)
{
switch (node->purpose)
{
case JOINT:
node->seqNumJoint = nJoint++;
node->seqNumEffector = -1;
break;
case EFFECTOR:
node->seqNumJoint = -1;
node->seqNumEffector = nEffector++;
break;
}
}
void Tree::InsertRoot(Node* root)
{
assert(nNode == 0);
nNode++;
Tree::root = root;
root->r = root->attach;
assert(!(root->left || root->right));
SetSeqNum(root);
}
void Tree::InsertLeftChild(Node* parent, Node* child)
{
assert(parent);
nNode++;
parent->left = child;
child->realparent = parent;
child->r = child->attach - child->realparent->attach;
assert(!(child->left || child->right));
SetSeqNum(child);
}
void Tree::InsertRightSibling(Node* parent, Node* child)
{
assert(parent);
nNode++;
parent->right = child;
child->realparent = parent->realparent;
child->r = child->attach - child->realparent->attach;
assert(!(child->left || child->right));
SetSeqNum(child);
}
// Search recursively below "node" for the node with index value.
Node* Tree::SearchJoint(Node* node, int index)
{
Node* ret;
if (node != 0)
{
if (node->seqNumJoint == index)
{
return node;
}
else
{
if ((ret = SearchJoint(node->left, index)))
{
return ret;
}
if ((ret = SearchJoint(node->right, index)))
{
return ret;
}
return NULL;
}
}
else
{
return NULL;
}
}
// Get the joint with the index value
Node* Tree::GetJoint(int index)
{
return SearchJoint(root, index);
}
// Search recursively below node for the end effector with the index value
Node* Tree::SearchEffector(Node* node, int index)
{
Node* ret;
if (node != 0)
{
if (node->seqNumEffector == index)
{
return node;
}
else
{
if ((ret = SearchEffector(node->left, index)))
{
return ret;
}
if ((ret = SearchEffector(node->right, index)))
{
return ret;
}
return NULL;
}
}
else
{
return NULL;
}
}
// Get the end effector for the index value
Node* Tree::GetEffector(int index)
{
return SearchEffector(root, index);
}
// Returns the global position of the effector.
const VectorR3& Tree::GetEffectorPosition(int index)
{
Node* effector = GetEffector(index);
assert(effector);
return (effector->s);
}
void Tree::ComputeTree(Node* node)
{
if (node != 0)
{
node->ComputeS();
node->ComputeW();
ComputeTree(node->left);
ComputeTree(node->right);
}
}
void Tree::Compute(void)
{
ComputeTree(root);
}
void Tree::PrintTree(Node* node)
{
if (node != 0)
{
node->PrintNode();
PrintTree(node->left);
PrintTree(node->right);
}
}
void Tree::Print(void)
{
PrintTree(root);
cout << "\n";
}
// Recursively initialize tree below the node
void Tree::InitTree(Node* node)
{
if (node != 0)
{
node->InitNode();
InitTree(node->left);
InitTree(node->right);
}
}
// Initialize all nodes in the tree
void Tree::Init(void)
{
InitTree(root);
}
void Tree::UnFreezeTree(Node* node)
{
if (node != 0)
{
node->UnFreeze();
UnFreezeTree(node->left);
UnFreezeTree(node->right);
}
}
void Tree::UnFreeze(void)
{
UnFreezeTree(root);
}

96
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/Tree.h Normal file
View file

@ -0,0 +1,96 @@
/*
*
* Inverse Kinematics software, with several solvers including
* Selectively Damped Least Squares Method
* Damped Least Squares Method
* Pure Pseudoinverse Method
* Jacobian Transpose Method
*
*
* Author: Samuel R. Buss, sbuss@ucsd.edu.
* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
*
*
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 "LinearR3.h"
#include "Node.h"
#ifndef _CLASS_TREE
#define _CLASS_TREE
class Tree
{
public:
Tree();
int GetNumNode() const { return nNode; }
int GetNumEffector() const { return nEffector; }
int GetNumJoint() const { return nJoint; }
void InsertRoot(Node*);
void InsertLeftChild(Node* parent, Node* child);
void InsertRightSibling(Node* parent, Node* child);
// Accessors based on node numbers
Node* GetJoint(int);
Node* GetEffector(int);
const VectorR3& GetEffectorPosition(int);
// Accessors for tree traversal
Node* GetRoot() const { return root; }
Node* GetSuccessor(const Node*) const;
Node* GetParent(const Node* node) const { return node->realparent; }
void Compute();
void Print();
void Init();
void UnFreeze();
private:
Node* root;
int nNode; // nNode = nEffector + nJoint
int nEffector;
int nJoint;
void SetSeqNum(Node*);
Node* SearchJoint(Node*, int);
Node* SearchEffector(Node*, int);
void ComputeTree(Node*);
void PrintTree(Node*);
void InitTree(Node*);
void UnFreezeTree(Node*);
};
inline Node* Tree::GetSuccessor(const Node* node) const
{
if (node->left)
{
return node->left;
}
while (true)
{
if (node->right)
{
return (node->right);
}
node = node->realparent;
if (!node)
{
return 0; // Back to root, finished traversal
}
}
}
#endif

48
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/VectorRn.cpp Normal file
View file

@ -0,0 +1,48 @@
/*
*
* 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.
*
*
*/
//
// VectorRn: Vector over Rn (Variable length vector)
//
#include "VectorRn.h"
VectorRn VectorRn::WorkVector;
double VectorRn::MaxAbs() const
{
double result = 0.0;
double* t = x;
for (long i = length; i > 0; i--)
{
if ((*t) > result)
{
result = *t;
}
else if (-(*t) > result)
{
result = -(*t);
}
t++;
}
return result;
}

275
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/VectorRn.h Normal file
View file

@ -0,0 +1,275 @@
/*
*
* 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.
*
*
*/
//
// VectorRn: Vector over Rn (Variable length vector)
//
// Not very sophisticated yet. Needs more functionality
// To do: better handling of resizing.
//
#ifndef VECTOR_RN_H
#define VECTOR_RN_H
#include <math.h>
#include <assert.h>
#include "LinearR3.h"
class VectorRn
{
friend class MatrixRmn;
public:
VectorRn(); // Null constructor
VectorRn(long length); // Constructor with length
~VectorRn(); // Destructor
void SetLength(long newLength);
long GetLength() const { return length; }
void SetZero();
void Fill(double d);
void Load(const double* d);
void LoadScaled(const double* d, double scaleFactor);
void Set(const VectorRn& src);
// Two access methods identical in functionality
// Subscripts are ZERO-BASED!!
const double& operator[](long i) const
{
assert(0 <= i && i < length);
return *(x + i);
}
double& operator[](long i)
{
assert(0 <= i && i < length);
return *(x + i);
}
double Get(long i) const
{
assert(0 <= i && i < length);
return *(x + i);
}
// Use GetPtr to get pointer into the array (efficient)
// Is friendly in that anyone can change the array contents (be careful!)
const double* GetPtr(long i) const
{
assert(0 <= i && i < length);
return (x + i);
}
double* GetPtr(long i)
{
assert(0 <= i && i < length);
return (x + i);
}
const double* GetPtr() const { return x; }
double* GetPtr() { return x; }
void Set(long i, double val) { assert(0 <= i && i < length), *(x + i) = val; }
void SetTriple(long i, const VectorR3& u);
VectorRn& operator+=(const VectorRn& src);
VectorRn& operator-=(const VectorRn& src);
void AddScaled(const VectorRn& src, double scaleFactor);
VectorRn& operator*=(double f);
double NormSq() const;
double Norm() const { return sqrt(NormSq()); }
double MaxAbs() const;
private:
long length; // Logical or actual length
long AllocLength; // Allocated length
double* x; // Array of vector entries
static VectorRn WorkVector; // Serves as a temporary vector
static VectorRn& GetWorkVector() { return WorkVector; }
static VectorRn& GetWorkVector(long len)
{
WorkVector.SetLength(len);
return WorkVector;
}
};
inline VectorRn::VectorRn()
{
length = 0;
AllocLength = 0;
x = 0;
}
inline VectorRn::VectorRn(long initLength)
{
length = 0;
AllocLength = 0;
x = 0;
SetLength(initLength);
}
inline VectorRn::~VectorRn()
{
delete[] x;
}
// Resize.
// If the array is shortened, the information about the allocated length is lost.
inline void VectorRn::SetLength(long newLength)
{
assert(newLength > 0);
if (newLength > AllocLength)
{
delete[] x;
AllocLength = Max(newLength, AllocLength << 1);
x = new double[AllocLength];
}
length = newLength;
}
// Zero out the entire vector
inline void VectorRn::SetZero()
{
double* target = x;
for (long i = length; i > 0; i--)
{
*(target++) = 0.0;
}
}
// Set the value of the i-th triple of entries in the vector
inline void VectorRn::SetTriple(long i, const VectorR3& u)
{
long j = 3 * i;
assert(0 <= j && j + 2 < length);
u.Dump(x + j);
}
inline void VectorRn::Fill(double d)
{
double* to = x;
for (long i = length; i > 0; i--)
{
*(to++) = d;
}
}
inline void VectorRn::Load(const double* d)
{
double* to = x;
for (long i = length; i > 0; i--)
{
*(to++) = *(d++);
}
}
inline void VectorRn::LoadScaled(const double* d, double scaleFactor)
{
double* to = x;
for (long i = length; i > 0; i--)
{
*(to++) = (*(d++)) * scaleFactor;
}
}
inline void VectorRn::Set(const VectorRn& src)
{
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for (long i = length; i > 0; i--)
{
*(to++) = *(from++);
}
}
inline VectorRn& VectorRn::operator+=(const VectorRn& src)
{
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for (long i = length; i > 0; i--)
{
*(to++) += *(from++);
}
return *this;
}
inline VectorRn& VectorRn::operator-=(const VectorRn& src)
{
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for (long i = length; i > 0; i--)
{
*(to++) -= *(from++);
}
return *this;
}
inline void VectorRn::AddScaled(const VectorRn& src, double scaleFactor)
{
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for (long i = length; i > 0; i--)
{
*(to++) += (*(from++)) * scaleFactor;
}
}
inline VectorRn& VectorRn::operator*=(double f)
{
double* target = x;
for (long i = length; i > 0; i--)
{
*(target++) *= f;
}
return *this;
}
inline double VectorRn::NormSq() const
{
double* target = x;
double res = 0.0;
for (long i = length; i > 0; i--)
{
res += (*target) * (*target);
target++;
}
return res;
}
inline double Dot(const VectorRn& u, const VectorRn& v)
{
assert(u.GetLength() == v.GetLength());
double res = 0.0;
const double* p = u.GetPtr();
const double* q = v.GetPtr();
for (long i = u.GetLength(); i > 0; i--)
{
res += (*(p++)) * (*(q++));
}
return res;
}
#endif //VECTOR_RN_H

14
Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/premake4.lua Normal file
View file

@ -0,0 +1,14 @@
project "BussIK"
kind "StaticLib"
includedirs {
"."
}
if os.is("Linux") then
buildoptions{"-fPIC"}
end
files {
"*.cpp",
"*.h",
}