691 lines
24 KiB
C++
691 lines
24 KiB
C++
/*
|
|
* Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
|
|
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
|
|
* All rights reserved. Email: russ@q12.org Web: www.q12.org
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "btSorLcp.h"
|
|
#include "btOdeSolverBody.h"
|
|
#include "LinearMath/btQuickProf.h"
|
|
|
|
#ifdef USE_SOR_SOLVER
|
|
|
|
// SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver.
|
|
#include "LinearMath/btScalar.h"
|
|
|
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
|
#include <math.h>
|
|
#include <float.h>//FLT_MAX
|
|
#ifdef WIN32
|
|
#include <memory.h>
|
|
#endif
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
#if defined (WIN32)
|
|
#include <malloc.h>
|
|
#else
|
|
#if defined (__FreeBSD__)
|
|
#include <stdlib.h>
|
|
#else
|
|
#include <alloca.h>
|
|
#endif
|
|
#endif
|
|
|
|
#include "btOdeJoint.h"
|
|
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
|
////////////////////////////////////////////////////////////////////
|
|
//math stuff
|
|
#include "btOdeMacros.h"
|
|
|
|
//***************************************************************************
|
|
// configuration
|
|
|
|
// for the SOR and CG methods:
|
|
// uncomment the following line to use warm starting. this definitely
|
|
// help for motor-driven joints. unfortunately it appears to hurt
|
|
// with high-friction contacts using the SOR method. use with care
|
|
|
|
//#define WARM_STARTING 1
|
|
|
|
// for the SOR method:
|
|
// uncomment the following line to randomly reorder constraint rows
|
|
// during the solution. depending on the situation, this can help a lot
|
|
// or hardly at all, but it doesn't seem to hurt.
|
|
|
|
//#define RANDOMLY_REORDER_CONSTRAINTS 1
|
|
|
|
//***************************************************************************
|
|
// various common computations involving the matrix JconstraintAxis
|
|
// compute iMJ = inv(M)*JconstraintAxis'
|
|
inline void compute_invM_JT (int numConstraintRows, dRealMutablePtr JconstraintAxis, dRealMutablePtr iMJ, int *jb,
|
|
//OdeSolverBody* const *body,
|
|
const btAlignedObjectArray<btOdeSolverBody*> &body,
|
|
dRealPtr inverseInertiaWorld)
|
|
{
|
|
int i,j;
|
|
dRealMutablePtr iMJ_ptr = iMJ;
|
|
dRealMutablePtr J_ptr = JconstraintAxis;
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
int b1 = jb[i*2];
|
|
int b2 = jb[i*2+1];
|
|
for (j=0; j<3; j++)
|
|
iMJ_ptr[j] = body[b1]->m_invMass*J_ptr[j];
|
|
dMULTIPLY0_331 (iMJ_ptr + 3, inverseInertiaWorld + 12*b1, J_ptr + 3);
|
|
|
|
if (b2 >= 0) {
|
|
for (j=0; j<3; j++)
|
|
iMJ_ptr[j+6] = body[b2]->m_invMass*J_ptr[j+6];//inv mass * constraint (normal) axis
|
|
dMULTIPLY0_331 (iMJ_ptr + 9, inverseInertiaWorld + 12*b2, J_ptr + 9);//inverse inertia world * constraint (normal) axis
|
|
}
|
|
J_ptr += 12;
|
|
iMJ_ptr += 12;
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
static void multiply_invM_JTSpecial (int numConstraintRows, int nb, dRealMutablePtr iMJ, int *jb,
|
|
dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2)
|
|
{
|
|
int i,j;
|
|
|
|
|
|
|
|
dRealMutablePtr out_ptr1 = out + onlyBody1*6;
|
|
|
|
for (j=0; j<6; j++)
|
|
out_ptr1[j] = 0;
|
|
|
|
if (onlyBody2 >= 0)
|
|
{
|
|
out_ptr1 = out + onlyBody2*6;
|
|
|
|
for (j=0; j<6; j++)
|
|
out_ptr1[j] = 0;
|
|
}
|
|
|
|
dRealPtr iMJ_ptr = iMJ;
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
|
|
int b1 = jb[i*2];
|
|
|
|
dRealMutablePtr out_ptr = out + b1*6;
|
|
if ((b1 == onlyBody1) || (b1 == onlyBody2))
|
|
{
|
|
for (j=0; j<6; j++)
|
|
out_ptr[j] += iMJ_ptr[j] * in[i] ;
|
|
}
|
|
|
|
iMJ_ptr += 6;
|
|
|
|
int b2 = jb[i*2+1];
|
|
if ((b2 == onlyBody1) || (b2 == onlyBody2))
|
|
{
|
|
if (b2 >= 0)
|
|
{
|
|
out_ptr = out + b2*6;
|
|
for (j=0; j<6; j++)
|
|
out_ptr[j] += iMJ_ptr[j] * in[i];
|
|
}
|
|
}
|
|
|
|
iMJ_ptr += 6;
|
|
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
// compute out = inv(M)*JconstraintAxis'*in.
|
|
|
|
#if 0
|
|
static void multiply_invM_JT (int numConstraintRows, int nb, dRealMutablePtr iMJ, int *jb,
|
|
dRealMutablePtr in, dRealMutablePtr out)
|
|
{
|
|
int i,j;
|
|
dSetZero1 (out,6*nb);
|
|
dRealPtr iMJ_ptr = iMJ;
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
int b1 = jb[i*2];
|
|
int b2 = jb[i*2+1];
|
|
dRealMutablePtr out_ptr = out + b1*6;
|
|
for (j=0; j<6; j++)
|
|
out_ptr[j] += iMJ_ptr[j] * in[i];
|
|
iMJ_ptr += 6;
|
|
if (b2 >= 0) {
|
|
out_ptr = out + b2*6;
|
|
for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
|
|
}
|
|
iMJ_ptr += 6;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
// compute out = JconstraintAxis*in.
|
|
inline void multiply_J (int numConstraintRows, dRealMutablePtr JconstraintAxis, int *jb,
|
|
dRealMutablePtr in, dRealMutablePtr out)
|
|
{
|
|
int i,j;
|
|
dRealPtr J_ptr = JconstraintAxis;
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
int b1 = jb[i*2];
|
|
int b2 = jb[i*2+1];
|
|
btScalar sum = 0;
|
|
dRealMutablePtr in_ptr = in + b1*6;
|
|
for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
|
|
J_ptr += 6;
|
|
if (b2 >= 0) {
|
|
in_ptr = in + b2*6;
|
|
for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
|
|
}
|
|
J_ptr += 6;
|
|
out[i] = sum;
|
|
}
|
|
}
|
|
|
|
//***************************************************************************
|
|
// SOR-LCP method
|
|
|
|
// nb is the number of bodies in the body array.
|
|
// JconstraintAxis is an numConstraintRows*12 matrix of constraint rows
|
|
// jb is an array of first and second body numbers for each constraint row
|
|
// inverseInertiaWorld is the global frame inverse inertia for each body (stacked 3x3 matrices)
|
|
//
|
|
// this returns lambdaAccumulatedImpulse and fc (the constraint force).
|
|
// note: fc is returned as inv(M)*JconstraintAxis'*lambdaAccumulatedImpulse, the constraint force is actually JconstraintAxis'*lambdaAccumulatedImpulse
|
|
//
|
|
// b, lowerLimit and higherLimit are modified on exit
|
|
|
|
//------------------------------------------------------------------------------
|
|
ATTRIBUTE_ALIGNED16(struct) IndexError {
|
|
btScalar error; // error to sort on
|
|
int findex;
|
|
int index; // row index
|
|
};
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btSorLcpSolver::SOR_LCP(int numConstraintRows, int nb, dRealMutablePtr JconstraintAxis, int *jb,
|
|
const btAlignedObjectArray<btOdeSolverBody*> &body,
|
|
dRealPtr inverseInertiaWorld, dRealMutablePtr lambdaAccumulatedImpulse, dRealMutablePtr invMforce, dRealMutablePtr rhs,
|
|
dRealMutablePtr lowerLimit, dRealMutablePtr higherLimit, dRealPtr cfm, int *findex,
|
|
int numiter,float overRelax,
|
|
btStackAlloc* stackAlloc
|
|
)
|
|
{
|
|
BT_PROFILE("btSorLcpSolver::SOR_LCP");
|
|
|
|
//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
|
|
AutoBlockSa asaBlock(stackAlloc);
|
|
|
|
const int num_iterations = numiter;
|
|
const float sor_w = overRelax; // SOR over-relaxation parameter
|
|
|
|
int i,j;
|
|
|
|
#ifdef WARM_STARTING
|
|
// for warm starting, this seems to be necessary to prevent
|
|
// jerkiness in motor-driven joints. i have no idea why this works.
|
|
for (i=0; i<numConstraintRows; i++) lambdaAccumulatedImpulse[i] *= 0.9;
|
|
#else
|
|
dSetZero1 (lambdaAccumulatedImpulse,numConstraintRows);
|
|
#endif
|
|
|
|
// the lambdaAccumulatedImpulse computed at the previous iteration.
|
|
// this is used to measure error for when we are reordering the indexes.
|
|
dRealAllocaArray (last_lambda,numConstraintRows);
|
|
|
|
// a copy of the 'higherLimit' vector in case findex[] is being used
|
|
dRealAllocaArray (hicopy,numConstraintRows);
|
|
memcpy (hicopy,higherLimit,numConstraintRows*sizeof(float));
|
|
|
|
// precompute iMJ = inv(M)*JconstraintAxis'
|
|
dRealAllocaArray (iMJ,numConstraintRows*12);
|
|
compute_invM_JT (numConstraintRows,JconstraintAxis,iMJ,jb,body,inverseInertiaWorld);
|
|
|
|
// compute fc=(inv(M)*JconstraintAxis')*lambdaAccumulatedImpulse. we will incrementally maintain fc
|
|
// as we change lambdaAccumulatedImpulse.
|
|
#ifdef WARM_STARTING
|
|
multiply_invM_JT (numConstraintRows,nb,iMJ,jb,lambdaAccumulatedImpulse,fc);
|
|
#else
|
|
dSetZero1 (invMforce,nb*6);
|
|
#endif
|
|
|
|
// precompute 1 / diagonals of A
|
|
dRealAllocaArray (Ad,numConstraintRows);
|
|
dRealPtr iMJ_ptr = iMJ;
|
|
dRealMutablePtr J_ptr = JconstraintAxis;
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
float sum = 0;
|
|
for (j=0; j<6; j++)
|
|
sum += iMJ_ptr[j] * J_ptr[j];
|
|
if (jb[i*2+1] >= 0) {
|
|
for (j=6; j<12; j++)
|
|
sum += iMJ_ptr[j] * J_ptr[j];
|
|
}
|
|
iMJ_ptr += 12;
|
|
J_ptr += 12;
|
|
Ad[i] = sor_w / sum;//(sum + cfm[i]);
|
|
}
|
|
|
|
// scale JconstraintAxis and b by Ad
|
|
J_ptr = JconstraintAxis;
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
for (j=0; j<12; j++) {
|
|
J_ptr[0] *= Ad[i];
|
|
J_ptr++;
|
|
}
|
|
rhs[i] *= Ad[i];
|
|
}
|
|
|
|
// scale Ad by CFM
|
|
for (i=0; i<numConstraintRows; i++)
|
|
Ad[i] *= cfm[i];
|
|
|
|
// order to solve constraint rows in
|
|
//IndexError *order = (IndexError*) alloca (numConstraintRows*sizeof(IndexError));
|
|
IndexError *order = (IndexError*) ALLOCA (numConstraintRows*sizeof(IndexError));
|
|
|
|
|
|
#ifndef REORDER_CONSTRAINTS
|
|
// make sure constraints with findex < 0 come first.
|
|
j=0;
|
|
for (i=0; i<numConstraintRows; i++)
|
|
if (findex[i] < 0)
|
|
order[j++].index = i;
|
|
for (i=0; i<numConstraintRows; i++)
|
|
if (findex[i] >= 0)
|
|
order[j++].index = i;
|
|
dIASSERT (j==numConstraintRows);
|
|
#endif
|
|
|
|
for (int iteration=0; iteration < num_iterations; iteration++) {
|
|
|
|
#ifdef REORDER_CONSTRAINTS
|
|
// constraints with findex < 0 always come first.
|
|
if (iteration < 2) {
|
|
// for the first two iterations, solve the constraints in
|
|
// the given order
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
order[i].error = i;
|
|
order[i].findex = findex[i];
|
|
order[i].index = i;
|
|
}
|
|
}
|
|
else {
|
|
// sort the constraints so that the ones converging slowest
|
|
// get solved last. use the absolute (not relative) error.
|
|
for (i=0; i<numConstraintRows; i++) {
|
|
float v1 = dFabs (lambdaAccumulatedImpulse[i]);
|
|
float v2 = dFabs (last_lambda[i]);
|
|
float max = (v1 > v2) ? v1 : v2;
|
|
if (max > 0) {
|
|
//@@@ relative error: order[i].error = dFabs(lambdaAccumulatedImpulse[i]-last_lambda[i])/max;
|
|
order[i].error = dFabs(lambdaAccumulatedImpulse[i]-last_lambda[i]);
|
|
}
|
|
else {
|
|
order[i].error = dInfinity;
|
|
}
|
|
order[i].findex = findex[i];
|
|
order[i].index = i;
|
|
}
|
|
}
|
|
qsort (order,numConstraintRows,sizeof(IndexError),&compare_index_error);
|
|
#endif
|
|
#ifdef RANDOMLY_REORDER_CONSTRAINTS
|
|
if ((iteration & 7) == 0) {
|
|
for (i=1; i<numConstraintRows; ++i) {
|
|
IndexError tmp = order[i];
|
|
int swapi = dRandInt2(i+1);
|
|
order[i] = order[swapi];
|
|
order[swapi] = tmp;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
//@@@ potential optimization: swap lambdaAccumulatedImpulse and last_lambda pointers rather
|
|
// than copying the data. we must make sure lambdaAccumulatedImpulse is properly
|
|
// returned to the caller
|
|
memcpy (last_lambda,lambdaAccumulatedImpulse,numConstraintRows*sizeof(float));
|
|
|
|
for (int i=0; i<numConstraintRows; i++) {
|
|
// @@@ potential optimization: we could pre-sort JconstraintAxis and iMJ, thereby
|
|
// linearizing access to those arrays. hmmm, this does not seem
|
|
// like a win, but we should think carefully about our memory
|
|
// access pattern.
|
|
|
|
int index = order[i].index;
|
|
J_ptr = JconstraintAxis + index*12;
|
|
iMJ_ptr = iMJ + index*12;
|
|
|
|
// set the limits for this constraint. note that 'hicopy' is used.
|
|
// this is the place where the QuickStep method differs from the
|
|
// direct LCP solving method, since that method only performs this
|
|
// limit adjustment once per time step, whereas this method performs
|
|
// once per iteration per constraint row.
|
|
// the constraints are ordered so that all lambdaAccumulatedImpulse[] values needed have
|
|
// already been computed.
|
|
if (findex[index] >= 0) {
|
|
higherLimit[index] = btFabs (hicopy[index] * lambdaAccumulatedImpulse[findex[index]]);
|
|
lowerLimit[index] = -higherLimit[index];
|
|
}
|
|
|
|
int b1 = jb[index*2];
|
|
int b2 = jb[index*2+1];
|
|
|
|
dRealMutablePtr deltaVelocity = invMforce + 6*b1;
|
|
|
|
float deltaAppliedImpulse = rhs[index] - lambdaAccumulatedImpulse[index]*Ad[index];
|
|
|
|
// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
|
|
deltaAppliedImpulse -=deltaVelocity[0] * J_ptr[0] + deltaVelocity[1] * J_ptr[1] +
|
|
deltaVelocity[2] * J_ptr[2] + deltaVelocity[3] * J_ptr[3] +
|
|
deltaVelocity[4] * J_ptr[4] + deltaVelocity[5] * J_ptr[5];
|
|
// @@@ potential optimization: handle 1-body constraints in a separate
|
|
// loop to avoid the cost of test & jump?
|
|
if (b2 >= 0) {
|
|
deltaVelocity = invMforce + 6*b2;
|
|
deltaAppliedImpulse -=deltaVelocity[0] * J_ptr[6] + deltaVelocity[1] * J_ptr[7] +
|
|
deltaVelocity[2] * J_ptr[8] + deltaVelocity[3] * J_ptr[9] +
|
|
deltaVelocity[4] * J_ptr[10] + deltaVelocity[5] * J_ptr[11];
|
|
}
|
|
|
|
// compute lambdaAccumulatedImpulse and clamp it to [lowerLimit,higherLimit].
|
|
// @@@ potential optimization: does SSE have clamping instructions
|
|
// to save test+jump penalties here?
|
|
float sum = lambdaAccumulatedImpulse[index] + deltaAppliedImpulse;
|
|
if (sum < lowerLimit[index]) {
|
|
deltaAppliedImpulse = lowerLimit[index]-lambdaAccumulatedImpulse[index];
|
|
lambdaAccumulatedImpulse[index] = lowerLimit[index];
|
|
}
|
|
else if (sum > higherLimit[index]) {
|
|
deltaAppliedImpulse = higherLimit[index]-lambdaAccumulatedImpulse[index];
|
|
lambdaAccumulatedImpulse[index] = higherLimit[index];
|
|
}
|
|
else {
|
|
lambdaAccumulatedImpulse[index] = sum;
|
|
}
|
|
|
|
//@@@ a trick that may or may not help
|
|
//float ramp = (1-((float)(iteration+1)/(float)num_iterations));
|
|
//deltaAppliedImpulse *= ramp;
|
|
|
|
// update invMforce.
|
|
// @@@ potential optimization: SIMD for this and the b2 >= 0 case
|
|
deltaVelocity = invMforce + 6*b1;
|
|
deltaVelocity[0] += deltaAppliedImpulse * iMJ_ptr[0];
|
|
deltaVelocity[1] += deltaAppliedImpulse * iMJ_ptr[1];
|
|
deltaVelocity[2] += deltaAppliedImpulse * iMJ_ptr[2];
|
|
deltaVelocity[3] += deltaAppliedImpulse * iMJ_ptr[3];
|
|
deltaVelocity[4] += deltaAppliedImpulse * iMJ_ptr[4];
|
|
deltaVelocity[5] += deltaAppliedImpulse * iMJ_ptr[5];
|
|
// @@@ potential optimization: handle 1-body constraints in a separate
|
|
// loop to avoid the cost of test & jump?
|
|
if (b2 >= 0) {
|
|
deltaVelocity = invMforce + 6*b2;
|
|
deltaVelocity[0] += deltaAppliedImpulse * iMJ_ptr[6];
|
|
deltaVelocity[1] += deltaAppliedImpulse * iMJ_ptr[7];
|
|
deltaVelocity[2] += deltaAppliedImpulse * iMJ_ptr[8];
|
|
deltaVelocity[3] += deltaAppliedImpulse * iMJ_ptr[9];
|
|
deltaVelocity[4] += deltaAppliedImpulse * iMJ_ptr[10];
|
|
deltaVelocity[5] += deltaAppliedImpulse * iMJ_ptr[11];
|
|
}
|
|
}
|
|
}
|
|
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btSorLcpSolver::SolveInternal1 (const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
|
|
btAlignedObjectArray<btOdeJoint*> &joint,
|
|
int nj, const btContactSolverInfo& solverInfo,
|
|
btStackAlloc* stackAlloc)
|
|
{
|
|
BT_PROFILE("btSorLcpSolver::SolveInternal1");
|
|
//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
|
|
AutoBlockSa asaBlock(stackAlloc);
|
|
|
|
int numIter = solverInfo.m_numIterations;
|
|
float sOr = solverInfo.m_sor;
|
|
|
|
int i,j;
|
|
|
|
btScalar stepsize1 = dRecip(solverInfo.m_timeStep);
|
|
|
|
// number all bodies in the body list - set their tag values
|
|
for (i=0; i<nb; i++)
|
|
body[i]->m_odeTag = i;
|
|
|
|
// make a local copy of the joint array, because we might want to modify it.
|
|
// (the "btOdeJoint *const*" declaration says we're allowed to modify the joints
|
|
// but not the joint array, because the caller might need it unchanged).
|
|
//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
|
|
//btOdeJoint **joint = (btOdeJoint**) alloca (nj * sizeof(btOdeJoint*));
|
|
//memcpy (joint,_joint,nj * sizeof(btOdeJoint*));
|
|
|
|
// for all bodies, compute the inertia tensor and its inverse in the global
|
|
// frame, and compute the rotational force and add it to the torque
|
|
// accumulator. I and inverseInertiaWorld are a vertical stack of 3x4 matrices, one per body.
|
|
dRealAllocaArray (I,3*4*nb);
|
|
dRealAllocaArray (inverseInertiaWorld,3*4*nb);
|
|
/* for (i=0; i<nb; i++) {
|
|
dMatrix3 tmp;
|
|
// compute inertia tensor in global frame
|
|
dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
|
|
// compute inverse inertia tensor in global frame
|
|
dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
|
|
dMULTIPLY0_333 (inverseInertiaWorld+i*12,body[i]->m_R,tmp);
|
|
// compute rotational force
|
|
dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
|
|
}
|
|
*/
|
|
for (i=0; i<nb; i++) {
|
|
dMatrix3 tmp;
|
|
// compute inertia tensor in global frame
|
|
dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
|
|
dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp);
|
|
|
|
// compute inverse inertia tensor in global frame
|
|
dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
|
|
dMULTIPLY0_333 (inverseInertiaWorld+i*12,body[i]->m_R,tmp);
|
|
// compute rotational force
|
|
// dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity);
|
|
// dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp);
|
|
}
|
|
|
|
|
|
|
|
|
|
// get joint information (numConstraintRows = total constraint dimension, nub = number of unbounded variables).
|
|
// joints with numConstraintRows=0 are inactive and are removed from the joints array
|
|
// entirely, so that the code that follows does not consider them.
|
|
//@@@ do we really need to save all the info1's
|
|
btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1));
|
|
|
|
for (i=0, j=0; j<nj; j++) { // i=dest, j=src
|
|
joint[j]->GetInfo1 (info+i);
|
|
dIASSERT (info[i].m_numConstraintRows >= 0 && info[i].m_numConstraintRows <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m_numConstraintRows);
|
|
if (info[i].m_numConstraintRows > 0) {
|
|
joint[i] = joint[j];
|
|
i++;
|
|
}
|
|
}
|
|
nj = i;
|
|
|
|
// create the row offset array
|
|
int numConstraintRows = 0;
|
|
int *constraintRowOffsets = (int*) ALLOCA (nj*sizeof(int));
|
|
for (i=0; i<nj; i++) {
|
|
constraintRowOffsets[i] = numConstraintRows;
|
|
numConstraintRows += info[i].m_numConstraintRows;
|
|
}
|
|
|
|
// if there are constraints, compute the constraint force
|
|
dRealAllocaArray (JconstraintAxis,numConstraintRows*12);
|
|
int *jb = (int*) ALLOCA (numConstraintRows*2*sizeof(int));
|
|
if (numConstraintRows > 0) {
|
|
// create a constraint equation right hand side vector `c', a constraint
|
|
// force mixing vector `cfm', and LCP low and high bound vectors, and an
|
|
// 'findex' vector.
|
|
dRealAllocaArray (c_rhs,numConstraintRows);
|
|
dRealAllocaArray (cfm,numConstraintRows);
|
|
dRealAllocaArray (lowerLimit,numConstraintRows);
|
|
dRealAllocaArray (higherLimit,numConstraintRows);
|
|
|
|
int *findex = (int*) ALLOCA (numConstraintRows*sizeof(int));
|
|
|
|
dSetZero1 (c_rhs,numConstraintRows);
|
|
dSetValue1 (cfm,numConstraintRows,solverInfo.m_globalCfm);
|
|
dSetValue1 (lowerLimit,numConstraintRows,-dInfinity);
|
|
dSetValue1 (higherLimit,numConstraintRows, dInfinity);
|
|
for (i=0; i<numConstraintRows; i++) findex[i] = -1;
|
|
|
|
// get jacobian data from constraints. an numConstraintRows*12 matrix will be created
|
|
// to store the two jacobian blocks from each constraint. it has this
|
|
// format:
|
|
//
|
|
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ .
|
|
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows)
|
|
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
|
|
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
|
|
// etc...
|
|
//
|
|
// (lll) = linear jacobian data
|
|
// (aaa) = angular jacobian data
|
|
//
|
|
dSetZero1 (JconstraintAxis,numConstraintRows*12);
|
|
btOdeJoint::Info2 Jinfo;
|
|
Jinfo.rowskip = 12;
|
|
Jinfo.fps = stepsize1;
|
|
Jinfo.erp = solverInfo.m_erp;
|
|
for (i=0; i<nj; i++) {
|
|
Jinfo.m_J1linearAxis = JconstraintAxis + constraintRowOffsets[i]*12;
|
|
Jinfo.m_J1angularAxis = Jinfo.m_J1linearAxis + 3;
|
|
Jinfo.m_J2linearAxis = Jinfo.m_J1linearAxis + 6;
|
|
Jinfo.m_J2angularAxis = Jinfo.m_J1linearAxis + 9;
|
|
Jinfo.m_constraintError = c_rhs + constraintRowOffsets[i];
|
|
Jinfo.cfm = cfm + constraintRowOffsets[i];
|
|
Jinfo.m_lowerLimit = lowerLimit + constraintRowOffsets[i];
|
|
Jinfo.m_higherLimit = higherLimit + constraintRowOffsets[i];
|
|
Jinfo.findex = findex + constraintRowOffsets[i];
|
|
joint[i]->GetInfo2 (&Jinfo);
|
|
|
|
if (Jinfo.m_constraintError[0] > solverInfo.m_maxErrorReduction)
|
|
Jinfo.m_constraintError[0] = solverInfo.m_maxErrorReduction;
|
|
|
|
// adjust returned findex values for global index numbering
|
|
for (j=0; j<info[i].m_numConstraintRows; j++) {
|
|
if (findex[constraintRowOffsets[i] + j] >= 0)
|
|
findex[constraintRowOffsets[i] + j] += constraintRowOffsets[i];
|
|
}
|
|
}
|
|
|
|
// create an array of body numbers for each joint row
|
|
int *jb_ptr = jb;
|
|
for (i=0; i<nj; i++) {
|
|
int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1;
|
|
int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1;
|
|
for (j=0; j<info[i].m_numConstraintRows; j++) {
|
|
jb_ptr[0] = b1;
|
|
jb_ptr[1] = b2;
|
|
jb_ptr += 2;
|
|
}
|
|
}
|
|
dIASSERT (jb_ptr == jb+2*numConstraintRows);
|
|
|
|
// compute the right hand side `rhs'
|
|
dRealAllocaArray (tmp1,nb*6);
|
|
// put v/h + invM*fe into tmp1
|
|
for (i=0; i<nb; i++) {
|
|
btScalar body_invMass = body[i]->m_invMass;
|
|
for (j=0; j<3; j++)
|
|
tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1;
|
|
dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,inverseInertiaWorld + i*12,body[i]->m_tacc);
|
|
for (j=0; j<3; j++)
|
|
tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1;
|
|
}
|
|
|
|
// put JconstraintAxis*tmp1 into rhs
|
|
dRealAllocaArray (rhs,numConstraintRows);
|
|
multiply_J (numConstraintRows,JconstraintAxis,jb,tmp1,rhs);
|
|
|
|
// complete rhs
|
|
for (i=0; i<numConstraintRows; i++)
|
|
rhs[i] = c_rhs[i]*stepsize1 - rhs[i];
|
|
|
|
// scale CFM
|
|
for (i=0; i<numConstraintRows; i++)
|
|
cfm[i] *= stepsize1;
|
|
|
|
// load lambdaAccumulatedImpulse from the value saved on the previous iteration
|
|
dRealAllocaArray (lambdaAccumulatedImpulse,numConstraintRows);
|
|
#ifdef WARM_STARTING
|
|
dSetZero1 (lambdaAccumulatedImpulse,numConstraintRows); //@@@ shouldn't be necessary
|
|
for (i=0; i<nj; i++) {
|
|
memcpy (lambdaAccumulatedImpulse+constraintRowOffsets[i],joint[i]->lambdaAccumulatedImpulse,info[i].numConstraintRows * sizeof(btScalar));
|
|
}
|
|
#endif
|
|
|
|
// solve the LCP problem and get lambdaAccumulatedImpulse and invM*constraint_force
|
|
dRealAllocaArray (cforce,nb*6);
|
|
|
|
/// SOR_LCP
|
|
SOR_LCP (numConstraintRows,nb,JconstraintAxis,jb,body,inverseInertiaWorld,lambdaAccumulatedImpulse,cforce,rhs,lowerLimit,higherLimit,cfm,findex,numIter,sOr,stackAlloc);
|
|
|
|
#ifdef WARM_STARTING
|
|
// save lambdaAccumulatedImpulse for the next iteration
|
|
//@@@ note that this doesn't work for contact joints yet, as they are
|
|
// recreated every iteration
|
|
for (i=0; i<nj; i++) {
|
|
memcpy (joint[i]->lambdaAccumulatedImpulse,lambdaAccumulatedImpulse+constraintRowOffsets[i],info[i].numConstraintRows * sizeof(btScalar));
|
|
}
|
|
#endif
|
|
|
|
// note that the SOR method overwrites rhs and JconstraintAxis at this point, so
|
|
// they should not be used again.
|
|
// add stepsize * cforce to the body velocity
|
|
for (i=0; i<nb; i++) {
|
|
for (j=0; j<3; j++)
|
|
body[i]->m_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j];
|
|
for (j=0; j<3; j++)
|
|
body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
|
|
|
|
}
|
|
}
|
|
|
|
// compute the velocity update:
|
|
// add stepsize * invM * fe to the body velocity
|
|
for (i=0; i<nb; i++) {
|
|
btScalar body_invMass = body[i]->m_invMass;
|
|
btVector3 linvel = body[i]->m_linearVelocity;
|
|
btVector3 angvel = body[i]->m_angularVelocity;
|
|
|
|
for (j=0; j<3; j++)
|
|
{
|
|
linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j];
|
|
}
|
|
for (j=0; j<3; j++)
|
|
{
|
|
body[i]->m_tacc[j] *= solverInfo.m_timeStep;
|
|
}
|
|
dMULTIPLY0_331NEW(angvel,+=,inverseInertiaWorld + i*12,body[i]->m_tacc);
|
|
body[i]->m_angularVelocity = angvel;
|
|
}
|
|
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
|
|
}
|
|
|
|
|
|
#endif //USE_SOR_SOLVER
|