Bart/bullet3
1
0 Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans committed 2018-09-23 14:17:31 -07:00
1 parent b73b05e9fb
commit ab8f16961e
1773 files changed
+1081067 -474229
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+285 -257
View File
@@ -30,7 +30,6 @@ subject to the following restrictions:
#include <iostream>
using namespace std;
#include "Jacobian.h"
void Arrow(const VectorR3& tail, const VectorR3& head);
@@ -39,15 +38,15 @@ void Arrow(const VectorR3& tail, const VectorR3& head);
//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 = 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::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)
@@ -55,86 +54,85 @@ Jacobian::Jacobian(Tree* tree)
m_tree = tree;
m_nEffector = tree->GetNumEffector();
nJoint = tree->GetNumJoint();
nRow = 3 * m_nEffector; // Include only the linear part
nRow = 3 * m_nEffector; // Include only the linear part
nCol = nJoint;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
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
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
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
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
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
Jacobian::Jacobian(bool useAngularJacobian,int nDof)
Jacobian::Jacobian(bool useAngularJacobian, int nDof)
{
m_tree = 0;
m_nEffector = 1;
if (useAngularJacobian)
{
nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
} else
nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
}
else
{
nRow = 3 * m_nEffector; // Include only the linear part
nRow = 3 * m_nEffector; // Include only the linear part
}
nCol = nDof;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
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
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
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
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
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
void Jacobian::Reset()
void Jacobian::Reset()
{
// Used by Damped Least Squares Method
DampingLambda = DefaultDampingLambda;
DampingLambdaSq = Square(DampingLambda);
// DampingLambdaSDLS = 1.5*DefaultDampingLambda;
dSclamp.Fill(HUGE_VAL);
}
@@ -142,14 +140,16 @@ void Jacobian::Reset()
// Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian(VectorR3* targets)
{
if (m_tree==0)
if (m_tree == 0)
return;
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
@@ -161,86 +161,92 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
// 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 ) {
while (m)
{
int j = m->GetJointNum();
assert ( 0 <=i && i<m_nEffector && 0<=j && j<nJoint );
if ( m->IsFrozen() ) {
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);
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 );
m = m_tree->GetParent(m);
}
}
n = m_tree->GetSuccessor( n );
n = m_tree->GetSuccessor(n);
}
}
void Jacobian::SetJendTrans(MatrixRmn& J)
{
Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
Jend.LoadAsSubmatrix(J);
Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
Jend.LoadAsSubmatrix(J);
}
void Jacobian::SetDeltaS(VectorRn& S)
{
dS.Set(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()
void Jacobian::UpdateThetas()
{
// Traverse the tree to find all joints
// Update the joint angles
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsJoint() ) {
while (n)
{
if (n->IsJoint())
{
int i = n->GetJointNum();
n->AddToTheta( dTheta[i] );
n->AddToTheta(dTheta[i]);
}
n = m_tree->GetSuccessor( n );
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)
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();
// 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()
void Jacobian::CalcDeltaThetas()
{
switch (CurrentUpdateMode) {
switch (CurrentUpdateMode)
{
case JACOB_Undefined:
ZeroDeltaThetas();
break;
@@ -270,162 +276,165 @@ void Jacobian::CalcDeltaThetasTranspose()
{
const MatrixRmn& J = ActiveJacobian();
J.MultiplyTranspose( dS, dTheta );
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 );
// 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;
double beta = MaxAngleJtranspose / maxChange;
dTheta *= Min(alpha, beta);
}
void Jacobian::CalcDeltaThetasPseudoinverse()
{
void Jacobian::CalcDeltaThetasPseudoinverse()
{
MatrixRmn& J = const_cast<MatrixRmn&>(ActiveJacobian());
// Compute Singular Value Decomposition
// 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 );
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w , V));
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();
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
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 );
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;
if (maxChange > MaxAnglePseudoinverse)
{
dTheta *= MaxAnglePseudoinverse / maxChange;
}
}
void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
{
{
const MatrixRmn& J = ActiveJacobian();
MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
U.AddToDiagonal( DampingLambdaSq );
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 );
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);
U.Solve(dS, &dT1);
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;
// Add null space velocity
dTheta += nullV;
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleDLS ) {
dTheta *= MaxAngleDLS/maxChange;
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
void Jacobian::CalcDeltaThetasDLS()
{
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 );
J.MultiplyTranspose( dT1, dTheta );
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleDLS ) {
dTheta *= MaxAngleDLS/maxChange;
}
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);
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)
{
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);
// 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
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);
// 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 );
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w , V));
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the DLS solution.
// Delta target values are the dS values
@@ -433,31 +442,32 @@ void Jacobian::CalcDeltaThetasDLSwithSVD()
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
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 );
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;
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
void Jacobian::CalcDeltaThetasSDLS()
{
void Jacobian::CalcDeltaThetasSDLS()
{
const MatrixRmn& J = ActiveJacobian();
// Compute Singular Value Decomposition
// Compute Singular Value Decomposition
J.ComputeSVD( U, w, V );
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w , V));
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
@@ -469,9 +479,10 @@ void Jacobian::CalcDeltaThetasSDLS()
// 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-- ) {
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++));
@@ -482,27 +493,29 @@ void Jacobian::CalcDeltaThetasSDLS()
CalcdTClampedFromdS();
// Loop over each singular vector
for ( i=0; i<nRows; i++ ) {
for (i = 0; i < nRows; i++)
{
double wiInv = w[i];
if ( NearZero(wiInv,1.0e-10) ) {
if (NearZero(wiInv, 1.0e-10))
{
continue;
}
wiInv = 1.0/wiInv;
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
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);
const double* dTx = dT1.GetPtr();
const double* ux = U.GetColumnPtr(i);
long j;
for ( j=numEndEffectors; j>0; j-- ) {
for (j = numEndEffectors; j > 0; j--)
{
double tmp;
alpha += (*ux)*(*(dTx++));
tmp = Square( *(ux++) );
alpha += (*ux)*(*(dTx++));
alpha += (*ux) * (*(dTx++));
tmp = Square(*(ux++));
alpha += (*ux) * (*(dTx++));
tmp += Square(*(ux++));
alpha += (*ux)*(*(dTx++));
alpha += (*ux) * (*(dTx++));
tmp += Square(*(ux++));
N += sqrt(tmp);
}
@@ -510,29 +523,32 @@ void Jacobian::CalcDeltaThetasSDLS()
// 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);
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-- ) {
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((*(vx++))) * accum;
}
M *= fabs(wiInv);
double gamma = MaxAngleSDLS;
if ( N<M ) {
gamma *= N/M; // Scale back maximum permissable joint angle
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 );
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);
double rescale = (gamma) / (gamma + max);
dTheta.AddScaled(dPreTheta, rescale);
/*if ( gamma<max) {
dTheta.AddScaled( dPreTheta, gamma/max );
}
@@ -543,28 +559,32 @@ void Jacobian::CalcDeltaThetasSDLS()
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleSDLS ) {
dTheta *= MaxAngleSDLS/(MaxAngleSDLS+maxChange);
if (maxChange > MaxAngleSDLS)
{
dTheta *= MaxAngleSDLS / (MaxAngleSDLS + maxChange);
//dTheta *= MaxAngleSDLS/maxChange;
}
}
void Jacobian::CalcdTClampedFromdS()
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;
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 {
else
{
dT1[i] = dS[i];
dT1[i+1] = dS[i+1];
dT1[i+2] = dS[i+2];
dT1[i + 1] = dS[i + 1];
dT1[i + 2] = dS[i + 2];
}
}
}
@@ -576,8 +596,10 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
temp = targetPos;
@@ -586,7 +608,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
errorArray[i] = err;
totalError += err;
}
n = m_tree->GetSuccessor( n );
n = m_tree->GetSuccessor(n);
}
return totalError;
}
@@ -596,8 +618,10 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
@@ -605,40 +629,44 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
// 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 ) {
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 {
else
{
dSclamp[i] = BaseMaxTargetDist;
}
}
n = m_tree->GetSuccessor( n );
n = m_tree->GetSuccessor(n);
}
}
void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 )
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++ ) {
for (long i = 0; i < len; i++)
{
double v1 = e1[i];
double v2 = e2[i];
if ( v1<v2 ) {
ret1 += v1/v2;
if (v1 < v2)
{
ret1 += v1 / v2;
ret2 += 1.0;
}
else if ( v1 != 0.0 ) {
else if (v1 != 0.0)
{
ret1 += 1.0;
ret2 += v2/v1;
ret2 += v2 / v1;
}
else {
else
{
ret1 += 0.0;
ret2 += 0.0;
}
@@ -647,22 +675,26 @@ void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* we
*weightedDist2 = ret2;
}
void Jacobian::CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies )
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 b1 = 0, b2 = 0, tie = 0;
int len = e1.GetLength();
for ( long i=0; i<len; i++ ) {
for (long i = 0; i < len; i++)
{
double v1 = e1[i];
double v2 = e2[i];
if ( v1<v2 ) {
if (v1 < v2)
{
b1++;
}
else if ( v2<v1 ) {
else if (v2 < v1)
{
b2++;
}
else {
else
{
tie++;
}
}
@@ -756,7 +788,3 @@ void Jacobian::CalcDeltaThetasSDLSrev2()
dTheta *= MaxAngleSDLS/maxChange;
}
} */
examples/ThirdPartyLibs/BussIK/Jacobian.h
+48 -42
View File
@@ -38,103 +38,109 @@ subject to the following restrictions:
#ifdef _DYNAMIC
const double BASEMAXDIST = 0.02;
#else
const double MAXDIST = 0.08; // optimal value for double Y shape : 0.08
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 long double LAMBDA = 2.0; // only for DLS. optimal : 0.24
const double NEARZERO = 0.0000000001;
enum UpdateMode {
enum UpdateMode
{
JACOB_Undefined = 0,
JACOB_JacobianTranspose = 1,
JACOB_PseudoInverse = 2,
JACOB_DLS = 3,
JACOB_SDLS = 4 };
JACOB_SDLS = 4
};
class Jacobian {
class Jacobian
{
public:
Jacobian(Tree*);
Jacobian(bool useAngularJacobian, int nDof);
void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
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 SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S);
void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
void ZeroDeltaThetas();
void CalcDeltaThetasTranspose();
void CalcDeltaThetasPseudoinverse();
void CalcDeltaThetasDLS();
void CalcDeltaThetasDLS2(const VectorRn& dVec);
void CalcDeltaThetasDLS2(const VectorRn& dVec);
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV);
void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV);
void UpdateThetas();
void UpdateThetaDot();
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
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; }
void SetCurrentMode(UpdateMode mode) { CurrentUpdateMode = mode; }
UpdateMode GetCurrentMode() const { return CurrentUpdateMode; }
void SetDampingDLS( double lambda ) { DampingLambda = lambda; DampingLambdaSq = Square(lambda); }
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 );
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; }
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)
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 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 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 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
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)
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;
static const double MaxAngleDLS;
static const double MaxAngleSDLS;
MatrixRmn* Jactive;
void CalcdTClampedFromdS();
static const double BaseMaxTargetDist;
};
#endif
examples/ThirdPartyLibs/BussIK/LinearR2.cpp
+19 -31
View File
@@ -1,4 +1,4 @@
/*
/*
*
* Mathematics Subpackage (VrMath)
*
@@ -22,7 +22,6 @@
#include "LinearR2.h"
#include <assert.h>
// ******************************************************
@@ -30,10 +29,10 @@
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
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::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 VectorR2 VectorR2::NegUnitY(0.0, -1.0);
const Matrix2x2 Matrix2x2::Identity(1.0, 0.0, 0.0, 1.0);
@@ -41,61 +40,50 @@ 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
LinearMapR2 LinearMapR2::Inverse() const // Returns inverse
{
double detInv = 1.0 / (m11 * m22 - m12 * m21);
double detInv = 1.0/(m11*m22 - m12*m21) ;
return( LinearMapR2( m22*detInv, -m21*detInv, -m12*detInv, m11*detInv ) );
return (LinearMapR2(m22 * detInv, -m21 * detInv, -m12 * detInv, m11 * detInv));
}
LinearMapR2& LinearMapR2::Invert() // Converts into inverse.
LinearMapR2& LinearMapR2::Invert() // Converts into inverse.
{
double detInv = 1.0/(m11*m22 - m12*m21) ;
double detInv = 1.0 / (m11 * m22 - m12 * m21);
double temp;
temp = m11*detInv;
m11= m22*detInv;
m22=temp;
m12 = -m12*detInv;
m21 = -m22*detInv;
temp = m11 * detInv;
m11 = m22 * detInv;
m22 = temp;
m12 = -m12 * detInv;
m21 = -m22 * detInv;
return ( *this );
return (*this);
}
VectorR2 LinearMapR2::Solve(const VectorR2& u) const // Returns solution
{
VectorR2 LinearMapR2::Solve(const VectorR2& u) const // Returns solution
{
// Just uses Inverse() for now.
return ( Inverse()*u );
return (Inverse() * u);
}
// ******************************************************
// * RotationMapR2 class - math library functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
// ***************************************************************
// * 2-space vector and matrix utilities *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
// ***************************************************************
// Stream Output Routines *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
ostream& operator<< ( ostream& os, const VectorR2& u )
ostream& operator<<(ostream& os, const VectorR2& u)
{
return (os << "<" << u.x << "," << u.y << ">");
}
examples/ThirdPartyLibs/BussIK/LinearR2.h
+474 -430
View File
File diff suppressed because it is too large. Load diff
examples/ThirdPartyLibs/BussIK/LinearR3.cpp
+458 -449
View File
File diff suppressed because it is too large. Load diff
examples/ThirdPartyLibs/BussIK/LinearR3.h
+1011 -911
View File
File diff suppressed because it is too large. Load diff
examples/ThirdPartyLibs/BussIK/LinearR4.cpp
+251 -251
View File
@@ -20,39 +20,43 @@ subject to the following restrictions:
*
*/
#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::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 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;
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;
}
@@ -60,90 +64,90 @@ double VectorR4::MaxAbs() const
// * Matrix4x4 class - math library functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
void Matrix4x4::operator*= (const Matrix4x4& B) // Matrix product
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;
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;
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;
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;
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 )
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);
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
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);
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);
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);
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);
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;
@@ -154,223 +158,221 @@ Matrix4x4& Matrix4x4::ReNormalize() {
// * LinearMapR4 class - math library functions *
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
double LinearMapR4::Determinant () const // Returns the determinant
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 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;
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 );
return (m11 * sd11 - m12 * sd12 + m13 * sd13 - m14 * sd14);
}
LinearMapR4 LinearMapR4::Inverse() const // Returns inverse
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 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 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);
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 ) );
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.
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 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 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);
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;
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 );
return (*this);
}
VectorR4 LinearMapR4::Solve(const VectorR4& u) const // Returns solution
{
VectorR4 LinearMapR4::Solve(const VectorR4& u) const // Returns solution
{
// Just uses Inverse() for now.
return ( Inverse()*u );
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 TimesTranspose(const VectorR4& u, const VectorR4& v)
{
LinearMapR4 result;
TimesTranspose( u, v, result );
TimesTranspose(u, v, result);
return result;
}
// The following routines are use to obtain
// 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 )
void GetOrtho(const VectorR4& u, RotationMapR4& rotmat)
{
rotmat.SetColumn1(u);
GetOrtho( 1, rotmat );
GetOrtho(1, rotmat);
}
void GetOrtho( const VectorR4& u, const VectorR4& v, RotationMapR4& rotmat )
void GetOrtho(const VectorR4& u, const VectorR4& v, RotationMapR4& rotmat)
{
rotmat.SetColumn1(u);
rotmat.SetColumn2(v);
GetOrtho( 2, rotmat );
GetOrtho(2, rotmat);
}
void GetOrtho( const VectorR4& u, const VectorR4& v, const VectorR4& s,
RotationMapR4& 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 );
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)
void GetOrtho(int j, RotationMapR4& rotmat)
{
if ( j==0 ) {
if (j == 0)
{
rotmat.SetIdentity();
return;
}
if ( j==1 ) {
rotmat.SetColumn2( -rotmat.m21, rotmat.m11, -rotmat.m41, rotmat.m31 );
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 );
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;
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);
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 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 );
else
{
vec3.Set(d34, 0.0, -d14, d13);
}
vec3.Normalize();
rotmat.SetColumn3(vec3);
@@ -378,90 +380,88 @@ void GetOrtho( int j, RotationMapR4& rotmat)
// 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 );
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 );
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 ) {
if (theta == 0.0)
{
return *this;
}
else {
else
{
theta = sqrt(theta);
double costheta = cos(theta);
double sintheta = sin(theta);
VectorR4 dirUnit = dir/theta;
*this = costheta*(*this) + sintheta*dirUnit;
VectorR4 dirUnit = dir / theta;
*this = costheta * (*this) + sintheta * dirUnit;
// this->NormalizeFast();
return ( *this );
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)
RotationMapR4 RotateToMap(const VectorR4& fromVec, const VectorR4& toVec)
{
LinearMapR4 result;
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 ) {
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.
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);
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);
VectorProjectMap(vPerp, temp); // Project in vPerp direction
temp *= (costheta - 1.0);
result += temp;
TimesTranspose ( vPerp, fromVec, temp ); // temp = (vPerp)*(fromVec^T)
TimesTranspose(vPerp, fromVec, temp); // temp = (vPerp)*(fromVec^T)
temp *= sintheta;
result += temp;
temp.MakeTranspose();
result -= temp; // (-sintheta)*(fromVec)*(vPerp^T)
result -= temp; // (-sintheta)*(fromVec)*(vPerp^T)
}
RotationMapR4 rotationResult;
rotationResult.Set(result); // Make explicitly a RotationMapR4
rotationResult.Set(result); // Make explicitly a RotationMapR4
return rotationResult;
}
// ***************************************************************
// Stream Output Routines *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
ostream& operator<< ( ostream& os, const VectorR4& u )
ostream& operator<<(ostream& os, const VectorR4& u)
{
return (os << "<" << u.x << "," << u.y << "," << u.z << "," << u.w << ">");
}
examples/ThirdPartyLibs/BussIK/LinearR4.h
+559 -490
View File
File diff suppressed because it is too large. Load diff
examples/ThirdPartyLibs/BussIK/MathMisc.h
+243 -180
View File
@@ -20,7 +20,6 @@ subject to the following restrictions:
*
*/
#ifndef MATH_MISC_H
#define MATH_MISC_H
@@ -31,41 +30,41 @@ subject to the following restrictions:
//
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 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 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 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 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);
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;
const double OnePlusEpsilon15 = 1.0 + 1.0e-15;
const double OneMinusEpsilon15 = 1.0 - 1.0e-15;
inline double ZeroValue(const double& x)
{
@@ -76,156 +75,191 @@ inline double ZeroValue(const double& x)
// Comparisons
//
template<class T> inline T Min ( T x, T y )
template <class T>
inline T Min(T x, T y)
{
return (x<y ? x : y);
return (x < y ? x : y);
}
template<class T> inline T Max ( T x, T y )
template <class T>
inline T Max(T x, T y)
{
return (y<x ? x : y);
return (y < x ? x : y);
}
template<class T> inline T ClampRange ( T x, T min, T max)
template <class T>
inline T ClampRange(T x, T min, T max)
{
if ( x<min ) {
if (x < min)
{
return min;
}
if ( x>max ) {
if (x > max)
{
return max;
}
return x;
}
template<class T> inline bool ClampRange ( T *x, T min, T max)
template <class T>
inline bool ClampRange(T* x, T min, T max)
{
if ( (*x)<min ) {
if ((*x) < min)
{
(*x) = min;
return false;
}
else if ( (*x)>max ) {
else if ((*x) > max)
{
(*x) = max;
return false;
}
else {
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 )
inline bool ClampMin(T* x, T min)
{
if ( x <= y ) {
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 );
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
{
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 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 (y > -1.0e-15); // x==0 in this case
}
}
else {
return ( -1.0e-15<x && x<1.0e-15 );
else
{
return (-1.0e-15 < x && x < 1.0e-15);
}
}
inline bool GreaterOrEqualFuzzy ( double x, double y )
inline bool GreaterOrEqualFuzzy(double x, double y)
{
return LessOrEqualFuzzy( y, x );
return LessOrEqualFuzzy(y, x);
}
inline bool UpdateMaxAbs( double *maxabs, double updateval )
inline bool UpdateMaxAbs(double* maxabs, double updateval)
{
if ( updateval > *maxabs ) {
if (updateval > *maxabs)
{
*maxabs = updateval;
return true;
}
else if ( -updateval > *maxabs ) {
else if (-updateval > *maxabs)
{
*maxabs = -updateval;
return true;
}
else {
else
{
return false;
}
}
@@ -235,33 +269,38 @@ inline bool UpdateMaxAbs( double *maxabs, double updateval )
// **********************************************************
template <class T>
void averageOf ( const T& a, const T &b, T&c ) {
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 ) {
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 *= alpha / beta;
c += a;
c *= beta;
}
else {
else
{
c = a;
c *= beta/alpha;
c *= beta / alpha;
c += b;
c *= alpha;
}
}
template <class T>
T Lerp( const T& a, const T&b, double alpha ) {
T Lerp(const T& a, const T& b, double alpha)
{
T ret;
Lerp( a, b, alpha, ret );
Lerp(a, b, alpha, ret);
return ret;
}
@@ -270,115 +309,139 @@ T Lerp( const T& a, const T&b, double alpha ) {
// **********************************************************
// 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;
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) );
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;
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;
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;
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);
else
{
return x / sin(x);
}
}
inline double SafeAsin( double x ) {
if ( x <= -1.0 ) {
inline double SafeAsin(double x)
{
if (x <= -1.0)
{
return -PIhalves;
}
else if ( x >= 1.0 ) {
else if (x >= 1.0)
{
return PIhalves;
}
else {
else
{
return asin(x);
}
}
inline double SafeAcos( double x ) {
if ( x <= -1.0 ) {
inline double SafeAcos(double x)
{
if (x <= -1.0)
{
return PI;
}
else if ( x >= 1.0 ) {
else if (x >= 1.0)
{
return 0.0;
}
else {
else
{
return acos(x);
}
}
// **********************************************************************
// Roots and powers *
// **********************************************************************
// Square(x) returns x*x, of course!
template<class T> inline T Square ( T x )
template <class T>
inline T Square(T x)
{
return (x*x);
return (x * x);
}
// Cube(x) returns x*x*x, of course!
template<class T> inline T Cube ( T x )
template <class T>
inline T Cube(T x)
{
return (x*x*x);
return (x * x * x);
}
// SafeSqrt(x) = returns sqrt(max(x, 0.0));
inline double SafeSqrt( double x ) {
if ( x<=0.0 ) {
inline double SafeSqrt(double x)
{
if (x <= 0.0)
{
return 0.0;
}
else {
else
{
return sqrt(x);
}
}
// SignedSqrt(a, s) returns (sign(s)*sqrt(a)).
inline double SignedSqrt( double a, double sgn )
inline double SignedSqrt(double a, double sgn)
{
if ( sgn==0.0 ) {
if (sgn == 0.0)
{
return 0.0;
}
else {
return ( sgn>0.0 ? sqrt(a) : -sqrt(a) );
else
{
return (sgn > 0.0 ? sqrt(a) : -sqrt(a));
}
}
// Template version of Sign function
template<class T> inline int Sign( T x)
template <class T>
inline int Sign(T x)
{
if ( x<0 ) {
if (x < 0)
{
return -1;
}
else if ( x==0 ) {
else if (x == 0)
{
return 0;
}
else {
else
{
return 1;
}
}
#endif // #ifndef MATH_MISC_H
#endif // #ifndef MATH_MISC_H
examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
+488 -418
View File
File diff suppressed because it is too large. Load diff
examples/ThirdPartyLibs/BussIK/MatrixRmn.h
+192 -175
View File
@@ -20,7 +20,6 @@ subject to the following restrictions:
*
*/
//
// MatrixRmn: Matrix over reals (Variable dimensional vector)
//
@@ -36,21 +35,21 @@ subject to the following restrictions:
#include "LinearR3.h"
#include "VectorRn.h"
class MatrixRmn {
class MatrixRmn
{
public:
MatrixRmn(); // Null constructor
MatrixRmn( long numRows, long numCols ); // Constructor with length
~MatrixRmn(); // Destructor
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();
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;
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!)
@@ -59,115 +58,124 @@ public:
// 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
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 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 );
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 );
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
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.
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);
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 ) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible.
void Solve(const VectorRn& b, VectorRn* x) 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
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.
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;
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;
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 );
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
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
static MatrixRmn WorkMatrix; // Temporary work matrix
static MatrixRmn WorkMatrix; // Temporary work matrix
static MatrixRmn& GetWorkMatrix() { return WorkMatrix; }
static MatrixRmn& GetWorkMatrix(long numRows, long numCols) { WorkMatrix.SetSize( numRows, numCols ); return WorkMatrix; }
static MatrixRmn& GetWorkMatrix(long numRows, long numCols)
{
WorkMatrix.SetSize(numRows, numCols);
return WorkMatrix;
}
// 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;
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()
inline MatrixRmn::MatrixRmn()
{
NumRows = 0;
NumCols = 0;
@@ -175,185 +183,192 @@ inline MatrixRmn::MatrixRmn()
AllocSize = 0;
}
inline MatrixRmn::MatrixRmn( long numRows, long numCols )
inline MatrixRmn::MatrixRmn(long numRows, long numCols)
{
NumRows = 0;
NumCols = 0;
x = 0;
AllocSize = 0;
SetSize( numRows, numCols );
SetSize(numRows, numCols);
}
inline MatrixRmn::~MatrixRmn()
inline MatrixRmn::~MatrixRmn()
{
delete[] x;
}
// Resize.
// Resize.
// If the array space is decreased, the information about the allocated length is lost.
inline void MatrixRmn::SetSize( long numRows, long numCols )
inline void MatrixRmn::SetSize(long numRows, long numCols)
{
assert ( numRows>0 && numCols>0 );
long newLength = numRows*numCols;
if ( newLength>AllocSize ) {
assert(numRows > 0 && numCols > 0);
long newLength = numRows * numCols;
if (newLength > AllocSize)
{
delete[] x;
AllocSize = Max(newLength, AllocSize<<1);
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-- ) {
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);
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
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 );
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.
// The entries are in column order.
// This version gives read-only pointer
inline const double* MatrixRmn::GetPtr( ) const
{
return x;
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;
// 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.
// 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);
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.
// 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);
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.
// 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);
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);
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.
// 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);
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);
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;
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 )
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 );
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 );
assert(NumRows == NumCols);
SetZero();
SetDiagonalEntries(1.0);
}
inline MatrixRmn& MatrixRmn::operator*=( double mult )
inline MatrixRmn& MatrixRmn::operator*=(double mult)
{
double* aPtr = x;
for ( long i=NumRows*NumCols; i>0; i-- ) {
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) *= mult;
}
return (*this);
}
inline MatrixRmn& MatrixRmn::AddScaled( const MatrixRmn& B, double factor )
inline MatrixRmn& MatrixRmn::AddScaled(const MatrixRmn& B, double factor)
{
assert (NumRows==B.NumRows && NumCols==B.NumCols);
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;
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) += (*(bPtr++)) * factor;
}
return (*this);
}
inline MatrixRmn& MatrixRmn::operator+=( const MatrixRmn& B )
inline MatrixRmn& MatrixRmn::operator+=(const MatrixRmn& B)
{
assert (NumRows==B.NumRows && NumCols==B.NumCols);
assert(NumRows == B.NumRows && NumCols == B.NumCols);
double* aPtr = x;
double* bPtr = B.x;
for ( long i=NumRows*NumCols; i>0; i-- ) {
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) += *(bPtr++);
}
return (*this);
}
inline MatrixRmn& MatrixRmn::operator-=( const MatrixRmn& B )
inline MatrixRmn& MatrixRmn::operator-=(const MatrixRmn& B)
{
assert (NumRows==B.NumRows && NumCols==B.NumCols);
assert(NumRows == B.NumRows && NumCols == B.NumCols);
double* aPtr = x;
double* bPtr = B.x;
for ( long i=NumRows*NumCols; i>0; i-- ) {
for (long i = NumRows * NumCols; i > 0; i--)
{
(*(aPtr++)) -= *(bPtr++);
}
return (*this);
@@ -363,18 +378,20 @@ inline double MatrixRmn::FrobeniusNormSq() const
{
double* aPtr = x;
double result = 0.0;
for ( long i=NumRows*NumCols; i>0; i-- ) {
result += Square( *(aPtr++) );
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 )
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);
for (; length > 0; length--)
{
result += (*ptrA) * (*ptrB);
ptrA += strideA;
ptrB += strideB;
}
@@ -382,26 +399,26 @@ inline double MatrixRmn::DotArray( long length, const double* ptrA, long strideA
}
// 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 )
inline void MatrixRmn::CopyArrayScale(long length, const double* from, long fromStride, double* to, long toStride, double scale)
{
for ( ; length>0; length-- ) {
*to = (*from)*scale;
for (; length > 0; length--)
{
*to = (*from) * scale;
from += fromStride;
to += toStride;
}
}
// Helper routine: adds a scaled array
// 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 )
inline void MatrixRmn::AddArrayScale(long length, const double* from, long fromStride, double* to, long toStride, double scale)
{
for ( ; length>0; length-- ) {
*to += (*from)*scale;
for (; length > 0; length--)
{
*to += (*from) * scale;
from += fromStride;
to += toStride;
}
}
#endif //MATRIX_RMN_H
#endif //MATRIX_RMN_H
examples/ThirdPartyLibs/BussIK/Misc.cpp
+21 -36
View File
@@ -66,9 +66,8 @@ static int zorder[] = {
1, 2, 3, 4, -5, 6
};
#endif
#define LENFRAC 0.10
#define BASEFRAC 1.10
#define LENFRAC 0.10
#define BASEFRAC 1.10
/****************************************************************
Arrow
@@ -76,15 +75,13 @@ static int zorder[] = {
/* size of wings as fraction of length: */
#define WINGS 0.10
#define WINGS 0.10
/* axes: */
#define X 1
#define Y 2
#define Z 3
#define X 1
#define Y 2
#define Z 3
/* x, y, z, axes: */
@@ -92,51 +89,39 @@ static int zorder[] = {
//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] );
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] )
float dot(float v1[3], float v2[3])
{
return( v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] );
return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
}
void
cross( float v1[3], float v2[3], float vout[3] )
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];
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 unit(float vin[3], float vout[3])
{
float dist, f ;
float dist, f;
dist = vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2];
dist = vin[0] * vin[0] + vin[1] * vin[1] + vin[2] * vin[2];
if( dist > 0.0 )
if (dist > 0.0)
{
dist = sqrt( dist );
dist = sqrt(dist);
f = 1. / dist;
vout[0] = f * vin[0];
vout[1] = f * vin[1];
@@ -149,5 +134,5 @@ unit( float vin[3], float vout[3] )
vout[2] = vin[2];
}
return( dist );
return (dist);
}
examples/ThirdPartyLibs/BussIK/Node.cpp
+10 -13
View File
@@ -20,10 +20,8 @@ subject to the following restrictions:
*
*/
#include <math.h>
#include "LinearR3.h"
#include "MathMisc.h"
#include "Node.h"
@@ -37,9 +35,9 @@ Node::Node(const VectorR3& attach, const VectorR3& v, double size, Purpose purpo
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
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;
@@ -52,9 +50,10 @@ 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 );
s = r; // Initialize to local (relative) position
while (y)
{
s.Rotate(y->theta, y->v);
y = y->realparent;
w = w->realparent;
s += w->r;
@@ -65,15 +64,14 @@ void Node::ComputeS(void)
void Node::ComputeW(void)
{
Node* y = this->realparent;
w = v; // Initialize to local rotation axis
while (y) {
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";
@@ -83,7 +81,6 @@ void Node::PrintNode()
cerr << "realparent : " << realparent->seqNumJoint << "\n";
}
void Node::InitNode()
{
theta = 0.0;
examples/ThirdPartyLibs/BussIK/Node.h
+40 -36
View File
@@ -20,31 +20,34 @@ subject to the following restrictions:
*
*/
#ifndef _CLASS_NODE
#define _CLASS_NODE
#include "LinearR3.h"
enum Purpose {JOINT, EFFECTOR};
enum Purpose
{
JOINT,
EFFECTOR
};
class VectorR3;
class Node {
class Node
{
friend class Tree;
public:
Node(const VectorR3&, const VectorR3&, double, Purpose, double minTheta=-PI, double maxTheta=PI, double restAngle=0.);
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 AddToTheta(double& delta)
{
//double orgTheta = theta;
theta += delta;
#if 0
@@ -55,24 +58,27 @@ public:
double actualDelta = theta - orgTheta;
delta = actualDelta;
#endif
return theta; }
double UpdateTheta( double& delta ) {
theta = delta;
return theta; }
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; } ;
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; }
bool IsEffector() const { return purpose == EFFECTOR; }
bool IsJoint() const { return purpose == JOINT; }
int GetEffectorNum() const { return seqNumEffector; }
int GetJointNum() const { return seqNumJoint; }
@@ -80,26 +86,24 @@ public:
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
//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
examples/ThirdPartyLibs/BussIK/Spherical.h
+119 -124
View File
@@ -20,8 +20,6 @@ subject to the following restrictions:
*
*/
//
// Spherical Operations Classes
//
@@ -30,7 +28,7 @@ subject to the following restrictions:
//
// OrientationR3
//
// A. Quaternion
// A. Quaternion
//
// B. RotationMapR3 // Elsewhere
//
@@ -48,80 +46,76 @@ subject to the following restrictions:
#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
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 {
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
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 );
SphericalInterpolator(const VectorR3& u, const VectorR3& v);
VectorR3 InterValue ( double frac ) const;
VectorR3 InterValue(double frac) const;
};
// ***************************************************************
// * Quaternion class - prototypes *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
class Quaternion {
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 );
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& );
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
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
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()
inline Quaternion ToQuat(const VectorR4& v)
{
return sqrt( x*x + y*y + z*z );
return Quaternion(v.x, v.y, v.z, v.w);
}
inline double Quaternion::Angle ()
inline double Quaternion::Norm()
{
return sqrt(x * x + y * y + z * z);
}
inline double Quaternion::Angle()
{
double halfAngle = asin(Norm());
return halfAngle+halfAngle;
return halfAngle + halfAngle;
}
// ****************************************************************
// Solid Geometry Routines *
// ****************************************************************
@@ -130,116 +124,118 @@ inline double Quaternion::Angle ()
// 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 );
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 )
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 );
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[] );
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.
// 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 );
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;
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 );
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 );
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 );
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 );
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 )
inline VectorR4::VectorR4(const Quaternion& q)
: x(q.x), y(q.y), z(q.z), w(q.w)
{
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 )
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;
@@ -248,7 +244,7 @@ inline Quaternion& Quaternion::Set( double xx, double yy, double zz, double ww )
return *this;
}
inline Quaternion& Quaternion::Set( const VectorR4& u)
inline Quaternion& Quaternion::Set(const VectorR4& u)
{
x = u.x;
y = u.y;
@@ -271,28 +267,27 @@ inline Quaternion operator*(const Quaternion& q1, const Quaternion& q2)
return q;
}
inline Quaternion& Quaternion::operator*=( const Quaternion& 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);
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
inline Quaternion Quaternion::Inverse() const // Return the Inverse
{
return ( Quaternion( x, y, z, -w ) );
return (Quaternion(x, y, z, -w));
}
inline Quaternion& Quaternion::Invert() // Invert this quaternion
inline Quaternion& Quaternion::Invert() // Invert this quaternion
{
w = -w;
return *this;
}
#endif // SPHERICAL_H
#endif // SPHERICAL_H
examples/ThirdPartyLibs/BussIK/Tree.cpp
+56 -42
View File
@@ -31,9 +31,6 @@ subject to the following restrictions:
#include <iostream>
using namespace std;
#include "LinearR3.h"
#include "Tree.h"
#include "Node.h"
@@ -46,25 +43,26 @@ Tree::Tree()
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;
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 );
assert(nNode == 0);
nNode++;
Tree::root = root;
root->r = root->attach;
assert( !(root->left || root->right) );
assert(!(root->left || root->right));
SetSeqNum(root);
}
@@ -75,7 +73,7 @@ void Tree::InsertLeftChild(Node* parent, Node* child)
parent->left = child;
child->realparent = parent;
child->r = child->attach - child->realparent->attach;
assert( !(child->left || child->right) );
assert(!(child->left || child->right));
SetSeqNum(child);
}
@@ -86,7 +84,7 @@ void Tree::InsertRightSibling(Node* parent, Node* child)
parent->right = child;
child->realparent = parent->realparent;
child->r = child->attach - child->realparent->attach;
assert( !(child->left || child->right) );
assert(!(child->left || child->right));
SetSeqNum(child);
}
@@ -94,25 +92,31 @@ void Tree::InsertRightSibling(Node* parent, Node* child)
Node* Tree::SearchJoint(Node* node, int index)
{
Node* ret;
if (node != 0) {
if (node->seqNumJoint == index) {
if (node != 0)
{
if (node->seqNumJoint == index)
{
return node;
} else {
if ((ret = SearchJoint(node->left, index))) {
}
else
{
if ((ret = SearchJoint(node->left, index)))
{
return ret;
}
if ((ret = SearchJoint(node->right, index))) {
if ((ret = SearchJoint(node->right, index)))
{
return ret;
}
return NULL;
}
}
else {
}
else
{
return NULL;
}
}
// Get the joint with the index value
Node* Tree::GetJoint(int index)
{
@@ -123,24 +127,31 @@ Node* Tree::GetJoint(int index)
Node* Tree::SearchEffector(Node* node, int index)
{
Node* ret;
if (node != 0) {
if (node->seqNumEffector == index) {
if (node != 0)
{
if (node->seqNumEffector == index)
{
return node;
} else {
if ((ret = SearchEffector(node->left, index))) {
}
else
{
if ((ret = SearchEffector(node->left, index)))
{
return ret;
}
if ((ret = SearchEffector(node->right, index))) {
if ((ret = SearchEffector(node->right, index)))
{
return ret;
}
return NULL;
}
} else {
}
else
{
return NULL;
}
}
// Get the end effector for the index value
Node* Tree::GetEffector(int index)
{
@@ -152,12 +163,13 @@ const VectorR3& Tree::GetEffectorPosition(int index)
{
Node* effector = GetEffector(index);
assert(effector);
return (effector->s);
return (effector->s);
}
void Tree::ComputeTree(Node* node)
{
if (node != 0) {
if (node != 0)
{
node->ComputeS();
node->ComputeW();
ComputeTree(node->left);
@@ -166,30 +178,31 @@ void Tree::ComputeTree(Node* node)
}
void Tree::Compute(void)
{
ComputeTree(root);
{
ComputeTree(root);
}
void Tree::PrintTree(Node* node)
{
if (node != 0) {
if (node != 0)
{
node->PrintNode();
PrintTree(node->left);
PrintTree(node->right);
}
}
void Tree::Print(void)
{
PrintTree(root);
void Tree::Print(void)
{
PrintTree(root);
cout << "\n";
}
// Recursively initialize tree below the node
void Tree::InitTree(Node* node)
{
if (node != 0) {
if (node != 0)
{
node->InitNode();
InitTree(node->left);
InitTree(node->right);
@@ -204,7 +217,8 @@ void Tree::Init(void)
void Tree::UnFreezeTree(Node* node)
{
if (node != 0) {
if (node != 0)
{
node->UnFreeze();
UnFreezeTree(node->left);
UnFreezeTree(node->right);
examples/ThirdPartyLibs/BussIK/Tree.h
+19 -15
View File
@@ -30,8 +30,8 @@ subject to the following restrictions:
#ifndef _CLASS_TREE
#define _CLASS_TREE
class Tree {
class Tree
{
public:
Tree();
@@ -49,43 +49,47 @@ public:
// Accessors for tree traversal
Node* GetRoot() const { return root; }
Node* GetSuccessor ( const Node* ) const;
Node* GetParent( const Node* node ) const { return node->realparent; }
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 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
inline Node* Tree::GetSuccessor(const Node* node) const
{
if ( node->left ) {
if (node->left)
{
return node->left;
}
while ( true ) {
if ( node->right ) {
return ( node->right );
while (true)
{
if (node->right)
{
return (node->right);
}
node = node->realparent;
if ( !node ) {
return 0; // Back to root, finished traversal
}
if (!node)
{
return 0; // Back to root, finished traversal
}
}
}
examples/ThirdPartyLibs/BussIK/VectorRn.cpp
+7 -5
View File
@@ -20,7 +20,6 @@ subject to the following restrictions:
*
*/
//
// VectorRn: Vector over Rn (Variable length vector)
//
@@ -29,15 +28,18 @@ subject to the following restrictions:
VectorRn VectorRn::WorkVector;
double VectorRn::MaxAbs () const
double VectorRn::MaxAbs() const
{
double result = 0.0;
double* t = x;
for ( long i = length; i>0; i-- ) {
if ( (*t) > result ) {
for (long i = length; i > 0; i--)
{
if ((*t) > result)
{
result = *t;
}
else if ( -(*t) > result ) {
else if (-(*t) > result)
{
result = -(*t);
}
t++;
examples/ThirdPartyLibs/BussIK/VectorRn.h
+114 -77
View File
@@ -34,205 +34,242 @@ subject to the following restrictions:
#include <assert.h>
#include "LinearR3.h"
class VectorRn {
class VectorRn
{
friend class MatrixRmn;
public:
VectorRn(); // Null constructor
VectorRn( long length ); // Constructor with length
~VectorRn(); // Destructor
VectorRn(); // Null constructor
VectorRn(long length); // Constructor with length
~VectorRn(); // Destructor
void SetLength( long newLength );
long GetLength() const { return length; }
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 );
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); }
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(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 );
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+=(const VectorRn& src);
VectorRn& operator-=(const VectorRn& src);
void AddScaled(const VectorRn& src, double scaleFactor);
VectorRn& operator*=( double f );
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
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; }
static VectorRn& GetWorkVector(long len)
{
WorkVector.SetLength(len);
return WorkVector;
}
};
inline VectorRn::VectorRn()
inline VectorRn::VectorRn()
{
length = 0;
AllocLength = 0;
x = 0;
}
inline VectorRn::VectorRn( long initLength )
inline VectorRn::VectorRn(long initLength)
{
length = 0;
AllocLength = 0;
x = 0;
SetLength( initLength );
SetLength(initLength);
}
inline VectorRn::~VectorRn()
inline VectorRn::~VectorRn()
{
delete[] x;
}
// Resize.
// Resize.
// If the array is shortened, the information about the allocated length is lost.
inline void VectorRn::SetLength( long newLength )
inline void VectorRn::SetLength(long newLength)
{
assert ( newLength>0 );
if ( newLength>AllocLength ) {
assert(newLength > 0);
if (newLength > AllocLength)
{
delete[] x;
AllocLength = Max( newLength, AllocLength<<1 );
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-- ) {
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 )
inline void VectorRn::SetTriple(long i, const VectorR3& u)
{
long j = 3*i;
assert ( 0<=j && j+2 < length );
u.Dump( x+j );
long j = 3 * i;
assert(0 <= j && j + 2 < length);
u.Dump(x + j);
}
inline void VectorRn::Fill( double d )
inline void VectorRn::Fill(double d)
{
double *to = x;
for ( long i=length; i>0; i-- ) {
double* to = x;
for (long i = length; i > 0; i--)
{
*(to++) = d;
}
}
inline void VectorRn::Load( const double* d )
inline void VectorRn::Load(const double* d)
{
double *to = x;
for ( long i=length; i>0; i-- ) {
double* to = x;
for (long i = length; i > 0; i--)
{
*(to++) = *(d++);
}
}
inline void VectorRn::LoadScaled( const double* d, double scaleFactor )
inline void VectorRn::LoadScaled(const double* d, double scaleFactor)
{
double *to = x;
for ( long i=length; i>0; i-- ) {
*(to++) = (*(d++))*scaleFactor;
double* to = x;
for (long i = length; i > 0; i--)
{
*(to++) = (*(d++)) * scaleFactor;
}
}
inline void VectorRn::Set( const VectorRn& src )
inline void VectorRn::Set(const VectorRn& src)
{
assert ( src.length == this->length );
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for ( long i=length; i>0; i-- ) {
for (long i = length; i > 0; i--)
{
*(to++) = *(from++);
}
}
inline VectorRn& VectorRn::operator+=( const VectorRn& src )
inline VectorRn& VectorRn::operator+=(const VectorRn& src)
{
assert ( src.length == this->length );
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for ( long i=length; i>0; i-- ) {
for (long i = length; i > 0; i--)
{
*(to++) += *(from++);
}
return *this;
}
inline VectorRn& VectorRn::operator-=( const VectorRn& src )
inline VectorRn& VectorRn::operator-=(const VectorRn& src)
{
assert ( src.length == this->length );
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for ( long i=length; i>0; i-- ) {
for (long i = length; i > 0; i--)
{
*(to++) -= *(from++);
}
return *this;
}
inline void VectorRn::AddScaled (const VectorRn& src, double scaleFactor )
inline void VectorRn::AddScaled(const VectorRn& src, double scaleFactor)
{
assert ( src.length == this->length );
assert(src.length == this->length);
double* to = x;
double* from = src.x;
for ( long i=length; i>0; i-- ) {
*(to++) += (*(from++))*scaleFactor;
for (long i = length; i > 0; i--)
{
*(to++) += (*(from++)) * scaleFactor;
}
}
inline VectorRn& VectorRn::operator*=( double f )
inline VectorRn& VectorRn::operator*=(double f)
{
double* target = x;
for ( long i=length; i>0; i-- ) {
for (long i = length; i > 0; i--)
{
*(target++) *= f;
}
return *this;
}
inline double VectorRn::NormSq() const
inline double VectorRn::NormSq() const
{
double* target = x;
double res = 0.0;
for ( long i=length; i>0; i-- ) {
res += (*target)*(*target);
for (long i = length; i > 0; i--)
{
res += (*target) * (*target);
target++;
}
return res;
}
inline double Dot( const VectorRn& u, const VectorRn& v )
inline double Dot(const VectorRn& u, const VectorRn& v)
{
assert ( u.GetLength() == v.GetLength() );
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++));
for (long i = u.GetLength(); i > 0; i--)
{
res += (*(p++)) * (*(q++));
}
return res;
}
#endif //VECTOR_RN_H
#endif //VECTOR_RN_H