void testRtoQandQtoR()
{
HEADER;
dMatrix3 R,I,R2;
dQuaternion q;
int i;
// test makeRandomRotation()
makeRandomRotation (R);
dMultiply2 (I,R,R,3,3,3);
printf ("\tmakeRandomRotation() - %s (1)\n",
cmpIdentityMat3(I) ? "passed" : "FAILED");
// test QtoR() on random normalized quaternions
int ok = 1;
for (i=0; i<100; i++) {
dMakeRandomVector (q,4,1.0);
dNormalize4 (q);
dQtoR (q,R);
dMultiply2 (I,R,R,3,3,3);
if (cmpIdentityMat3(I)==0) ok = 0;
}
printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED");
// test R -> Q -> R works
dReal maxdiff=0;
for (i=0; i<100; i++) {
makeRandomRotation (R);
dRtoQ (R,q);
dQtoR (q,R2);
dReal diff = dMaxDifference (R,R2,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s (3)\n",maxdiff,
(maxdiff > tol) ? "FAILED" : "passed");
}
void dxStepBody (dxBody *b, dReal h)
{
// cap the angular velocity
if (b->flags & dxBodyMaxAngularSpeed) {
const dReal max_ang_speed = b->max_angular_speed;
const dReal aspeed = dCalcVectorDot3( b->avel, b->avel );
if (aspeed > max_ang_speed*max_ang_speed) {
const dReal coef = max_ang_speed/dSqrt(aspeed);
dScaleVector3(b->avel, coef);
}
}
// end of angular velocity cap
// handle linear velocity
for (unsigned int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (unsigned int j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->posr.R);
// notify all attached geoms that this body has moved
dxWorldProcessContext *world_process_context = b->world->UnsafeGetWorldProcessingContext();
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) {
world_process_context->LockForStepbodySerialization();
dGeomMoved (geom);
world_process_context->UnlockForStepbodySerialization();
}
// notify the user
if (b->moved_callback != NULL) {
b->moved_callback(b);
}
// damping
if (b->flags & dxBodyLinearDamping) {
const dReal lin_threshold = b->dampingp.linear_threshold;
const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel );
if ( lin_speed > lin_threshold) {
const dReal k = 1 - b->dampingp.linear_scale;
dScaleVector3(b->lvel, k);
}
}
if (b->flags & dxBodyAngularDamping) {
const dReal ang_threshold = b->dampingp.angular_threshold;
const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel );
if ( ang_speed > ang_threshold) {
const dReal k = 1 - b->dampingp.angular_scale;
dScaleVector3(b->avel, k);
}
}
}
void dxStepBody (dxBody *b, dReal h)
{
int j;
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
// handle linear velocity
for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->R);
// notify all attached geoms that this body has moved
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
}
static inline void
moveAndRotateBody (dxBody * b, dReal h)
{
int j;
// handle linear velocity
for (j = 0; j < 3; j++)
b->posr.pos[j] += dMUL(h,b->lvel[j]);
if (b->flags & dxBodyFlagFiniteRotation)
{
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis, b->avel);
frv[0] = dMUL(b->finite_rot_axis[0],k);
frv[1] = dMUL(b->finite_rot_axis[1],k);
frv[2] = dMUL(b->finite_rot_axis[2],k);
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(k,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(frv[0],s);
q[2] = dMUL(frv[1],s);
q[3] = dMUL(frv[2],s);
}
else
{
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (dMUL(b->avel[0],b->avel[0]) + dMUL(b->avel[1],b->avel[1]) + dMUL(b->avel[2],b->avel[2]));
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(wlen,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(b->avel[0],s);
q[2] = dMUL(b->avel[1],s);
q[3] = dMUL(b->avel[2],s);
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2, q, b->q);
for (j = 0; j < 4; j++)
b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
dReal dq[4];
dWtoDQ (irv, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
}
else
{
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q, b->posr.R);
// notify all attached geoms that this body has moved
for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
}
void reset_test()
{
int i;
dMass m,anchor_m;
dReal q[NUM][3], pm[NUM]; // particle positions and masses
dReal pos1[3] = {1,0,1}; // point of reference (POR)
dReal pos2[3] = {-1,0,1}; // point of reference (POR)
// make random particle positions (relative to POR) and masses
for (i=0; i<NUM; i++) {
pm[i] = dRandReal()+0.1;
q[i][0] = dRandReal()-0.5;
q[i][1] = dRandReal()-0.5;
q[i][2] = dRandReal()-0.5;
}
// adjust particle positions so centor of mass = POR
computeMassParams (&m,q,pm);
for (i=0; i<NUM; i++) {
q[i][0] -= m.c[0];
q[i][1] -= m.c[1];
q[i][2] -= m.c[2];
}
if (world) dWorldDestroy (world);
world = dWorldCreate();
anchor_body = dBodyCreate (world);
dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
dMassAdjust (&anchor_m,0.1);
dBodySetMass (anchor_body,&anchor_m);
for (i=0; i<NUM; i++) {
particle[i] = dBodyCreate (world);
dBodySetPosition (particle[i],
pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,pm[i]);
dBodySetMass (particle[i],&m);
}
for (i=0; i < NUM; i++) {
particle_joint[i] = dJointCreateBall (world,0);
dJointAttach (particle_joint[i],anchor_body,particle[i]);
const dReal *p = dBodyGetPosition (particle[i]);
dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
}
// make test_body with the same mass and inertia of the anchor_body plus
// all the particles
test_body = dBodyCreate (world);
dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
computeMassParams (&m,q,pm);
m.mass += anchor_m.mass;
for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
dBodySetMass (test_body,&m);
// rotate the test and anchor bodies by a random amount
dQuaternion qrot;
for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
dNormalize4 (qrot);
dBodySetQuaternion (anchor_body,qrot);
dBodySetQuaternion (test_body,qrot);
dMatrix3 R;
dQtoR (qrot,R);
for (i=0; i<NUM; i++) {
dVector3 v;
dMultiply0 (v,R,&q[i][0],3,3,1);
dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
}
// set random torque
for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;
iteration=0;
}
// Ripped from Opcode 1.1.
static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, float& u, float& v){
//calculate plane of triangle
dVector4 Plane;
dCROSS(Plane, =, Edge0, Edge1);
Plane[3] = dDOT(Plane, Origin);
//normalize
dNormalize4(Plane);
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
float side = dDOT(Plane,Center) - Plane[3];
if(side < 0.0f) {
return false;
}
// now onto the bulk of the collision...
dVector3 Diff;
Diff[0] = Origin[0] - Center[0];
Diff[1] = Origin[1] - Center[1];
Diff[2] = Origin[2] - Center[2];
Diff[3] = Origin[3] - Center[3];
float A00 = dDOT(Edge0, Edge0);
float A01 = dDOT(Edge0, Edge1);
float A11 = dDOT(Edge1, Edge1);
float B0 = dDOT(Diff, Edge0);
float B1 = dDOT(Diff, Edge1);
float C = dDOT(Diff, Diff);
float Det = dFabs(A00 * A11 - A01 * A01);
u = A01 * B1 - A11 * B0;
v = A01 * B0 - A00 * B1;
float DistSq;
if (u + v <= Det){
if(u < REAL(0.0)){
if(v < REAL(0.0)){ // region 4
if(B0 < REAL(0.0)){
v = REAL(0.0);
if (-B0 >= A00){
u = REAL(1.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = -B0 / A00;
DistSq = B0 * u + C;
}
}
else{
u = REAL(0.0);
if(B1 >= REAL(0.0)){
v = REAL(0.0);
DistSq = C;
}
else if(-B1 >= A11){
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
v = -B1 / A11;
DistSq = B1 * v + C;
}
}
}
else{ // region 3
u = REAL(0.0);
if(B1 >= REAL(0.0)){
v = REAL(0.0);
DistSq = C;
}
else if(-B1 >= A11){
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
v = -B1 / A11;
DistSq = B1 * v + C;
}
}
}
else if(v < REAL(0.0)){ // region 5
v = REAL(0.0);
if (B0 >= REAL(0.0)){
u = REAL(0.0);
DistSq = C;
}
else if (-B0 >= A00){
u = REAL(1.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = -B0 / A00;
DistSq = B0 * u + C;
}
}
else{ // region 0
// minimum at interior point
if (Det == REAL(0.0)){
u = REAL(0.0);
v = REAL(0.0);
DistSq = FLT_MAX;
}
else{
float InvDet = REAL(1.0) / Det;
u *= InvDet;
v *= InvDet;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
}
else{
float Tmp0, Tmp1, Numer, Denom;
if(u < REAL(0.0)){ // region 2
Tmp0 = A01 + B0;
Tmp1 = A11 + B1;
if (Tmp1 > Tmp0){
Numer = Tmp1 - Tmp0;
Denom = A00 - REAL(2.0) * A01 + A11;
if (Numer >= Denom){
u = REAL(1.0);
v = REAL(0.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = Numer / Denom;
v = REAL(1.0) - u;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
else{
u = REAL(0.0);
if(Tmp1 <= REAL(0.0)){
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else if(B1 >= REAL(0.0)){
v = REAL(0.0);
DistSq = C;
}
else{
v = -B1 / A11;
DistSq = B1 * v + C;
}
}
}
else if(v < REAL(0.0)){ // region 6
Tmp0 = A01 + B1;
Tmp1 = A00 + B0;
if (Tmp1 > Tmp0){
Numer = Tmp1 - Tmp0;
Denom = A00 - REAL(2.0) * A01 + A11;
if (Numer >= Denom){
v = REAL(1.0);
u = REAL(0.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
v = Numer / Denom;
u = REAL(1.0) - v;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
else{
v = REAL(0.0);
if (Tmp1 <= REAL(0.0)){
u = REAL(1.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else if(B0 >= REAL(0.0)){
u = REAL(0.0);
DistSq = C;
}
else{
u = -B0 / A00;
DistSq = B0 * u + C;
}
}
}
else{ // region 1
Numer = A11 + B1 - A01 - B0;
if (Numer <= REAL(0.0)){
u = REAL(0.0);
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
Denom = A00 - REAL(2.0) * A01 + A11;
if (Numer >= Denom){
u = REAL(1.0);
v = REAL(0.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = Numer / Denom;
v = REAL(1.0) - u;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
}
}
Dist = dSqrt(dFabs(DistSq));
if (Dist <= Radius){
Dist = Radius - Dist;
return true;
}
else return false;
}
