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); }