dxGeom::dxGeom (dSpaceID _space, int is_placeable)
{
initColliders();
// setup body vars. invalid type of -1 must be changed by the constructor.
type = -1;
gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED;
if (is_placeable) gflags |= GEOM_PLACEABLE;
data = 0;
body = 0;
body_next = 0;
if (is_placeable) {
dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
pos = pr->pos;
R = pr->R;
dSetZero (pos,4);
dRSetIdentity (R);
}
else {
pos = 0;
R = 0;
}
// setup space vars
next = 0;
tome = 0;
parent_space = 0;
dSetZero (aabb,6);
category_bits = ~0;
collide_bits = ~0;
// put this geom in a space if required
if (_space) dSpaceAdd (_space,this);
}
dxJoint::dxJoint( dxWorld *w ) :
dObject( w )
{
//printf("constructing %p\n", this);
dIASSERT( w );
flags = 0;
node[0].joint = this;
node[0].body = 0;
node[0].next = 0;
node[1].joint = this;
node[1].body = 0;
node[1].next = 0;
dSetZero( lambda, 6 );
dSetZero( lambda_erp, 6 );
addObjectToList( this, ( dObject ** ) &w->firstjoint );
w->nj++;
feedback = 0;
// Moved here by OSRF
// Default to negative value, which means the current global value
// will be used. If set non-negative, then this joint-specific
// value will be used.
erp = -1; // world->global_erp;
cfm = -1; // world->global_cfm;
}
int _dInvertPDMatrix (const dReal *A, dReal *Ainv, int n, void *tmpbuf/*[nskip*(n+2)]*/)
{
dAASSERT (n > 0 && A && Ainv);
bool success = false;
size_t FactorCholesky_size = _dEstimateFactorCholeskyTmpbufSize(n);
size_t SolveCholesky_size = _dEstimateSolveCholeskyTmpbufSize(n);
size_t MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;
dIASSERT(MaxCholesky_size % sizeof(dReal) == 0);
const int nskip = dPAD (n);
const int nskip_mul_n = nskip*n;
dReal *tmp = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA (MaxCholesky_size + (nskip + nskip_mul_n)*sizeof(dReal));
dReal *X = (dReal *)((char *)tmp + MaxCholesky_size);
dReal *L = X + nskip;
memcpy (L, A, nskip_mul_n*sizeof(dReal));
if (dFactorCholesky (L,n,tmp)) {
dSetZero (Ainv,nskip_mul_n); // make sure all padding elements set to 0
dReal *aa = Ainv, *xi = X, *xiend = X + n;
for (; xi != xiend; ++aa, ++xi) {
dSetZero(X, n);
*xi = REAL(1.0);
dSolveCholesky (L,X,n,tmp);
dReal *a = aa;
const dReal *x = X, *xend = X + n;
for (; x!=xend; a+=nskip, ++x) {
*a = *x;
}
}
success = true;
}
return success ? 1 : 0;
}
void testMatrixMultiply()
{
// A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4
dReal A[8],B[12],A2[12],B2[16],C[8];
int i;
HEADER;
dSetZero (A,8);
for (i=0; i<3; i++) A[i] = i+2;
for (i=0; i<3; i++) A[i+4] = i+3+2;
for (i=0; i<12; i++) B[i] = i+8;
dSetZero (A2,12);
for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3];
dSetZero (B2,16);
for (i=0; i<12; i++) B2[i+i/3] = B[i];
dMultiply0 (C,A,B,2,3,4);
if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 ||
C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278)
printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n");
dMultiply1 (C,A2,B,2,3,4);
if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 ||
C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241)
printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n");
dMultiply2 (C,A,B2,2,3,4);
if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 ||
C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326)
printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n");
}
dxGeom::dxGeom (dSpaceID _space, int is_placeable)
{
// setup body vars. invalid type of -1 must be changed by the constructor.
type = -1;
gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED;
if (is_placeable) gflags |= GEOM_PLACEABLE;
data = 0;
body = 0;
body_next = 0;
if (is_placeable) {
final_posr = dAllocPosr();
dSetZero (final_posr->pos,4);
dRSetIdentity (final_posr->R);
}
else {
final_posr = 0;
}
offset_posr = 0;
// setup space vars
next = 0;
tome = 0;
next_ex = 0;
tome_ex = 0;
parent_space = 0;
dSetZero (aabb,6);
category_bits = ~0;
collide_bits = ~0;
// put this geom in a space if required
if (_space) dSpaceAdd (_space,this);
}
void dMassSetZero (dMass *m)
{
dAASSERT (m);
m->mass = REAL(0.0);
dSetZero (m->c,sizeof(m->c) / sizeof(dReal));
dSetZero (m->I,sizeof(m->I) / sizeof(dReal));
}
dxJointBall::dxJointBall( dxWorld *w ) :
dxJoint( w )
{
dSetZero( anchor1, 4 );
dSetZero( anchor2, 4 );
erp = world->global_erp;
cfm = world->global_cfm;
}
dxJointFixed::dxJointFixed ( dxWorld *w ) :
dxJoint ( w )
{
dSetZero ( offset, 4 );
dSetZero ( qrel, 4 );
erp = world->global_erp;
cfm = world->global_cfm;
}
dxJointFixed::dxJointFixed ( dxWorld *w ) :
dxJoint ( w )
{
dSetZero ( offset, 4 );
dSetZero ( qrel, 4 );
// These are now set in dxJoint constructor
// erp = world->global_erp;
// cfm = world->global_cfm;
}
dxJointDBall::dxJointDBall(dxWorld *w) :
dxJoint(w)
{
dSetZero(anchor1, 3);
dSetZero(anchor2, 3);
targetDistance = 0;
erp = world->global_erp;
cfm = world->global_cfm;
}
dxJointSlider::dxJointSlider ( dxWorld *w ) :
dxJoint ( w )
{
dSetZero ( axis1, 4 );
axis1[0] = 1;
dSetZero ( qrel, 4 );
dSetZero ( offset, 4 );
limot.init ( world );
}
dxJointGearbox::dxJointGearbox(dxWorld* w) :
dxJoint(w)
{
ratio = 1.0;
flags |= dJOINT_TWOBODIES;
dSetZero( qrel1, 4 );
dSetZero( qrel2, 4 );
cumulative_angle1 = 0.0;
cumulative_angle2 = 0.0;
}
dxPlanarJoint::dxPlanarJoint(dxWorld *world) :
dxJoint(world)
{
dSetZero(anchor, 3);
dCopyVector3(anchor1, anchor);
dCopyVector3(anchor2, anchor);
// default to plane z=0
dSetZero(planeNormal, 3);
planeNormal[2] = 1;
dCopyVector3(axis1, planeNormal);
dCopyVector3(axis2, planeNormal);
}
dxJointHinge::dxJointHinge( dxWorld *w ) :
dxJoint( w )
{
dSetZero( anchor1, 4 );
dSetZero( anchor2, 4 );
dSetZero( axis1, 4 );
axis1[0] = 1;
dSetZero( axis2, 4 );
axis2[0] = 1;
dSetZero( qrel, 4 );
limot.init( world );
cumulative_angle = 0;
}
dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
dReal *_Dell, dReal *_ell, dReal *_tmp,
int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
{
dUASSERT (_findex==0,"slow dLCP object does not support findex array");
n = _n;
nub = _nub;
Adata = _Adata;
A = 0;
x = _x;
b = _b;
w = _w;
lo = _lo;
hi = _hi;
nskip = dPAD(n);
dSetZero (x,n);
last_i_for_solve1 = -1;
int i,j;
C.setSize (n);
N.setSize (n);
for (int i=0; i<n; i++) {
C[i] = 0;
N[i] = 0;
}
# ifdef ROWPTRS
// make matrix row pointers
A = Arows;
for (i=0; i<n; i++) A[i] = Adata + i*nskip;
# else
A = Adata;
# endif
// lets make A symmetric
for (i=0; i<n; i++) {
for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i];
}
// if nub>0, put all indexes 0..nub-1 into C and solve for x
if (nub > 0) {
for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal));
dFactorLDLT (_L,_d,nub,nskip);
memcpy (x,b,nub*sizeof(dReal));
dSolveLDLT (_L,_d,x,nub,nskip);
dSetZero (_w,nub);
for (i=0; i<nub; i++) C[i] = 1;
}
}
dxJointScrew::dxJointScrew( dxWorld *w ) :
dxJoint( w )
{
dSetZero( anchor1, 4 );
dSetZero( anchor2, 4 );
dSetZero( axis1, 4 );
axis1[0] = 1;
dSetZero( axis2, 4 );
axis2[0] = 1;
dSetZero( qrel, 4 );
limot.init( world );
cumulative_angle = 0;
thread_pitch = 1.0; // rad/m (3141.6 rad/m is about 0.002 m/rev)
}
void testLDLTAddTL()
{
int i,j;
dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], a[MSIZE],
DL[MSIZE4*MSIZE], ATEST[MSIZE4*MSIZE], diff;
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
dFactorLDLT (L,d,MSIZE,MSIZE4);
// delete first row and column of factorization
for (i=0; i<MSIZE; i++) a[i] = -A[i*MSIZE4];
a[0] += 1;
dLDLTAddTL (L,d,a,MSIZE,MSIZE4);
for (i=1; i<MSIZE; i++) L[i*MSIZE4] = 0;
d[0] = 1;
// get modified L*D*L'
dClearUpperTriangle (L,MSIZE);
for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
dSetZero (DL,MSIZE4*MSIZE);
for (i=0; i<MSIZE; i++) {
for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
}
dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
// compare it to A with its first row/column removed
for (i=1; i<MSIZE; i++) A[i*MSIZE4] = A[i] = 0;
A[0] = 1;
diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
printf ("\tmaximum difference = %.6e - %s\n",diff,
diff > tol ? "FAILED" : "passed");
}
void testInvertPDMatrix()
{
int i,j,ok;
dReal A[MSIZE4*MSIZE], Ainv[MSIZE4*MSIZE], I[MSIZE4*MSIZE];
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal));
dSetZero (Ainv,MSIZE4*MSIZE);
if (dInvertPDMatrix (A,Ainv,MSIZE))
printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n");
dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE);
// compare with identity
ok = 1;
for (i=0; i<MSIZE; i++) {
for (j=0; j<MSIZE; j++) {
if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0;
}
}
for (i=0; i<MSIZE; i++) {
if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0;
}
if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n");
}
dxJointAMotor::dxJointAMotor( dxWorld *w ) :
dxJoint( w )
{
int i;
num = 0;
mode = dAMotorUser;
for ( i = 0; i < 3; i++ )
{
rel[i] = 0;
dSetZero( axis[i], 4 );
limot[i].init( world );
angle[i] = 0;
}
dSetZero( reference1, 4 );
dSetZero( reference2, 4 );
}
dMatrix::dMatrix (int rows, int cols)
{
if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
n = rows;
m = cols;
data = (dReal*) dAlloc (n*m*sizeof(dReal));
dSetZero (data,n*m);
}
dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1)
{
type = dGeomTransformClass;
obj = 0;
cleanup = 0;
infomode = 0;
dSetZero (final_pos,4);
dRSetIdentity (final_R);
}
dReal cmpIdentity (const dMatrix3 A)
{
dMatrix3 I;
dSetZero (I,12);
I[0] = 1;
I[5] = 1;
I[10] = 1;
return dMaxDifference (A,I,3,3);
}
void dMassTranslate (dMass *m, dReal x, dReal y, dReal z)
{
// if the body is translated by `a' relative to its point of reference,
// the new inertia about the point of reference is:
//
// I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
//
// where c is the existing center of mass and I is the old inertia.
int i,j;
dMatrix3 ahat,chat,t1,t2;
dReal a[3];
dAASSERT (m);
// adjust inertia matrix
dSetZero (chat,12);
dSetCrossMatrixPlus (chat,m->c,4);
a[0] = x + m->c[0];
a[1] = y + m->c[1];
a[2] = z + m->c[2];
dSetZero (ahat,12);
dSetCrossMatrixPlus (ahat,a,4);
dMultiply0_333 (t1,ahat,ahat);
dMultiply0_333 (t2,chat,chat);
for (i=0; i<3; i++) for (j=0; j<3; j++)
m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);
// ensure perfect symmetry
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
// adjust center of mass
m->c[0] += x;
m->c[1] += y;
m->c[2] += z;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
//****************************************************************************
// lmotor joint
dxJointLMotor::dxJointLMotor( dxWorld *w ) :
dxJoint( w )
{
int i;
num = 0;
for ( i = 0;i < 3;i++ )
{
dSetZero( axis[i], 4 );
limot[i].init( world );
}
}
void testSetZero()
{
HEADER;
dReal a[100];
dMakeRandomVector (a,100,1.0);
dSetZero (a,100);
for (int i=0; i<100; i++) if (a[i] != 0.0) {
printf ("\tFAILED\n");
return;
}
printf ("\tpassed\n");
}
dxJointPR::dxJointPR( dxWorld *w ) :
dxJoint( w )
{
// Default Position
// Z^
// | Body 1 P R Body2
// |+---------+ _ _ +-----------+
// || |----|----(_)--------+ |
// |+---------+ - +-----------+
// |
// X.-----------------------------------------> Y
// N.B. X is comming out of the page
dSetZero( anchor2, 4 );
dSetZero( axisR1, 4 );
axisR1[0] = 1;
dSetZero( axisR2, 4 );
axisR2[0] = 1;
dSetZero( axisP1, 4 );
axisP1[1] = 1;
dSetZero( qrel, 4 );
dSetZero( offset, 4 );
limotR.init( world );
limotP.init( world );
}
dxJointHinge2::dxJointHinge2( dxWorld *w ) :
dxJoint( w )
{
dSetZero( anchor1, 4 );
dSetZero( anchor2, 4 );
dSetZero( axis1, 4 );
axis1[0] = 1;
dSetZero( axis2, 4 );
axis2[1] = 1;
c0 = 0;
s0 = 0;
dSetZero( v1, 4 );
v1[0] = 1;
dSetZero( v2, 4 );
v2[1] = 1;
limot1.init( world );
limot2.init( world );
susp_erp = world->global_erp;
susp_cfm = world->global_cfm;
flags |= dJOINT_TWOBODIES;
}
void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result)
{
dAASSERT (g);
if (g->offset_posr)
{
dRtoQ (g->offset_posr->R, result);
}
else
{
dSetZero (result,4);
result[0] = 1;
}
}
dxJointTransmission::dxJointTransmission(dxWorld* w) :
dxJoint(w)
{
int i;
flags |= dJOINT_TWOBODIES;
mode = dTransmissionParallelAxes;
cfm = world->global_cfm;
erp = world->global_erp;
for (i = 0 ; i < 2 ; i += 1) {
dSetZero( anchors[i], 4 );
dSetZero( axes[i], 4 );
axes[i][0] = 1;
radii[i] = 0;
}
backlash = 0;
ratio = 1;
update = 1;
}
void _InitCylinderTrimeshData(sData& cData)
{
// get cylinder information
// Rotation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);
// Position
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
// Cylinder axis
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get trimesh position and orientation
const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
dMatrix3Copy(pRotTris,cData.mTrimeshRot);
dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);
// Position
const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
dVector3Copy(*pPosTris,cData.vTrimeshPos);
// calculate basic angle for 8-gon
dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
// calculate angle increment
dReal fAngleIncrement = fAngle*REAL(2.0);
// calculate plane normals
// axis dependant code
for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = REAL(0.0);
fAngle += fAngleIncrement;
}
dSetZero(cData.vBestPoint,4);
// reset best depth
cData.fBestCenter = REAL(0.0);
}
