Menu
▾
▴
[Opende-svn] SF.net SVN: opende:[1923] trunk/ode/src/joints
|
From: <ole...@us...> - 2012-12-30 15:55:09
|
Revision: 1923
http://opende.svn.sourceforge.net/opende/?rev=1923&view=rev
Author: oleh_derevenko
Date: 2012-12-30 15:55:01 +0000 (Sun, 30 Dec 2012)
Log Message:
-----------
Cosmetic: Minor code improvements to allow better optimizations by compiler
Modified Paths:
--------------
trunk/ode/src/joints/contact.cpp
trunk/ode/src/joints/fixed.cpp
trunk/ode/src/joints/joint.cpp
Modified: trunk/ode/src/joints/contact.cpp
===================================================================
--- trunk/ode/src/joints/contact.cpp 2012-12-29 11:46:13 UTC (rev 1922)
+++ trunk/ode/src/joints/contact.cpp 2012-12-30 15:55:01 UTC (rev 1923)
@@ -98,20 +98,24 @@
// c1,c2 = contact points with respect to body PORs
dVector3 c1, c2 = {0,0,0};
- c1[0] = contact.geom.pos[0] - node[0].body->posr.pos[0];
- c1[1] = contact.geom.pos[1] - node[0].body->posr.pos[1];
- c1[2] = contact.geom.pos[2] - node[0].body->posr.pos[2];
+ dxBody *b0 = node[0].body;
+ c1[0] = contact.geom.pos[0] - b0->posr.pos[0];
+ c1[1] = contact.geom.pos[1] - b0->posr.pos[1];
+ c1[2] = contact.geom.pos[2] - b0->posr.pos[2];
+
// set jacobian for normal
info->J1l[0] = normal[0];
info->J1l[1] = normal[1];
info->J1l[2] = normal[2];
dCalcVectorCross3( info->J1a, c1, normal );
- if ( node[1].body )
+
+ dxBody *b1 = node[1].body;
+ if ( b1 )
{
- c2[0] = contact.geom.pos[0] - node[1].body->posr.pos[0];
- c2[1] = contact.geom.pos[1] - node[1].body->posr.pos[1];
- c2[2] = contact.geom.pos[2] - node[1].body->posr.pos[2];
+ c2[0] = contact.geom.pos[0] - b1->posr.pos[0];
+ c2[1] = contact.geom.pos[1] - b1->posr.pos[1];
+ c2[2] = contact.geom.pos[2] - b1->posr.pos[2];
info->J2l[0] = -normal[0];
info->J2l[1] = -normal[1];
info->J2l[2] = -normal[2];
@@ -195,6 +199,7 @@
info->J1l[s+1] = t1[1];
info->J1l[s+2] = t1[2];
dCalcVectorCross3( info->J1a + s, c1, t1 );
+
if ( node[1].body )
{
info->J2l[s+0] = -t1[0];
@@ -204,6 +209,7 @@
dCalcVectorCross3( J2a_plus_s, c2, t1 );
dNegateVector3( J2a_plus_s );
}
+
// set right hand side
if ( contact.surface.mode & dContactMotion1 )
{
@@ -235,6 +241,7 @@
info->J1l[s2+1] = t2[1];
info->J1l[s2+2] = t2[2];
dCalcVectorCross3( info->J1a + s2, c1, t2 );
+
if ( node[1].body )
{
info->J2l[s2+0] = -t2[0];
@@ -244,11 +251,13 @@
dCalcVectorCross3( J2a_plus_s2, c2, t2 );
dNegateVector3( J2a_plus_s2 );
}
+
// set right hand side
if ( contact.surface.mode & dContactMotion2 )
{
info->c[rowFriction2] = contact.surface.motion2;
}
+
// set LCP bounds and friction index. this depends on the approximation
// mode
info->lo[rowFriction2] = -mu2;
Modified: trunk/ode/src/joints/fixed.cpp
===================================================================
--- trunk/ode/src/joints/fixed.cpp 2012-12-29 11:46:13 UTC (rev 1922)
+++ trunk/ode/src/joints/fixed.cpp 2012-12-30 15:55:01 UTC (rev 1923)
@@ -74,9 +74,12 @@
info->cfm[1] = this->cfm;
info->cfm[2] = this->cfm;
+ dxBody *b0 = node[0].body, *b1 = node[1].body;
+
dVector3 ofs;
- dMultiply0_331 ( ofs, node[0].body->posr.R, offset );
- if ( node[1].body )
+ dMultiply0_331 ( ofs, b0->posr.R, offset );
+
+ if ( b1 )
{
dSetCrossMatrixPlus( info->J1a, ofs, s );
info->J2l[0] = -1;
@@ -86,17 +89,21 @@
// set right hand side for the first three rows (linear)
dReal k = worldFPS * this->erp;
- if ( node[1].body )
+ if ( b1 )
{
for ( int j = 0; j < 3; j++ )
- info->c[j] = k * ( node[1].body->posr.pos[j]
- - node[0].body->posr.pos[j]
+ {
+ info->c[j] = k * ( b1->posr.pos[j]
+ - b0->posr.pos[j]
+ ofs[j] );
+ }
}
else
{
for ( int j = 0; j < 3; j++ )
- info->c[j] = k * ( offset[j] - node[0].body->posr.pos[j] );
+ {
+ info->c[j] = k * ( offset[j] - b0->posr.pos[j] );
+ }
}
}
Modified: trunk/ode/src/joints/joint.cpp
===================================================================
--- trunk/ode/src/joints/joint.cpp 2012-12-29 11:46:13 UTC (rev 1922)
+++ trunk/ode/src/joints/joint.cpp 2012-12-30 15:55:01 UTC (rev 1923)
@@ -116,23 +116,26 @@
info->J1l[2*s+2] = 1;
dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
dSetCrossMatrixMinus( info->J1a, a1, s );
- if ( joint->node[1].body )
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
{
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
- dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
+ dMultiply0_331( a2, b1->posr.R, anchor2 );
dSetCrossMatrixPlus( info->J2a, a2, s );
}
// set right hand side
dReal k = fps * erp;
- if ( joint->node[1].body )
+ dxBody *b0 = joint->node[0].body;
+ if ( b1 )
{
for ( int j = 0; j < 3; j++ )
{
- info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] -
- a1[j] - joint->node[0].body->posr.pos[j] );
+ info->c[j] = k * ( a2[j] + b1->posr.pos[j] -
+ a1[j] - b0->posr.pos[j] );
}
}
else
@@ -140,7 +143,7 @@
for ( int j = 0; j < 3; j++ )
{
info->c[j] = k * ( anchor2[j] - a1[j] -
- joint->node[0].body->posr.pos[j] );
+ b0->posr.pos[j] );
}
}
}
@@ -173,12 +176,14 @@
dCalcVectorCross3( info->J1a, a1, axis );
dCalcVectorCross3( info->J1a + s, a1, q1 );
dCalcVectorCross3( info->J1a + 2*s, a1, q2 );
- if ( joint->node[1].body )
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
{
for ( i = 0; i < 3; i++ ) info->J2l[i] = -axis[i];
for ( i = 0; i < 3; i++ ) info->J2l[s+i] = -q1[i];
for ( i = 0; i < 3; i++ ) info->J2l[2*s+i] = -q2[i];
- dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
+ dMultiply0_331( a2, b1->posr.R, anchor2 );
dReal *J2a = info->J2a;
dCalcVectorCross3( J2a, a2, axis );
dNegateVector3( J2a );
@@ -194,11 +199,19 @@
dReal k1 = fps * erp1;
dReal k = fps * erp;
- for ( i = 0; i < 3; i++ ) a1[i] += joint->node[0].body->posr.pos[i];
- if ( joint->node[1].body )
+ dxBody *b0 = joint->node[0].body;
+ for ( i = 0; i < 3; i++ )
{
- for ( i = 0; i < 3; i++ ) a2[i] += joint->node[1].body->posr.pos[i];
+ a1[i] += b0->posr.pos[i];
+ }
+ if ( b1 )
+ {
+ for ( i = 0; i < 3; i++ )
+ {
+ a2[i] += b1->posr.pos[i];
+ }
+
dVector3 a2_minus_a1;
dSubtractVectors3(a2_minus_a1, a2, a1);
info->c[0] = k1 * dCalcVectorDot3( axis, a2_minus_a1 );
@@ -228,7 +241,9 @@
info->J1a[start_index] = 1;
info->J1a[start_index + s + 1] = 1;
info->J1a[start_index + s*2+2] = 1;
- if ( joint->node[1].body )
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
{
info->J2a[start_index] = -1;
info->J2a[start_index + s+1] = -1;
@@ -251,15 +266,16 @@
// get qerr = relative rotation (rotation error) between two bodies
dQuaternion qerr, e;
- if ( joint->node[1].body )
+ dxBody *b0 = joint->node[0].body;
+ if ( b1 )
{
dQuaternion qq;
- dQMultiply1( qq, joint->node[0].body->q, joint->node[1].body->q );
+ dQMultiply1( qq, b0->q, b1->q );
dQMultiply2( qerr, qq, qrel );
}
else
{
- dQMultiply3( qerr, joint->node[0].body->q, qrel );
+ dQMultiply3( qerr, b0->q, qrel );
}
if ( qerr[0] < 0 )
{
@@ -267,7 +283,7 @@
qerr[2] = -qerr[2];
qerr[3] = -qerr[3];
}
- dMultiply0_331( e, joint->node[0].body->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
+ dMultiply0_331( e, b0->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
dReal k = fps * erp;
info->c[start_row] = 2 * k * e[0];
info->c[start_row+1] = 2 * k * e[1];
@@ -280,21 +296,24 @@
void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
dVector3 anchor1, dVector3 anchor2 )
{
- if ( j->node[0].body )
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
{
dReal q[4];
- q[0] = x - j->node[0].body->posr.pos[0];
- q[1] = y - j->node[0].body->posr.pos[1];
- q[2] = z - j->node[0].body->posr.pos[2];
+ q[0] = x - b0->posr.pos[0];
+ q[1] = y - b0->posr.pos[1];
+ q[2] = z - b0->posr.pos[2];
q[3] = 0;
- dMultiply1_331( anchor1, j->node[0].body->posr.R, q );
- if ( j->node[1].body )
+ dMultiply1_331( anchor1, b0->posr.R, q );
+
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
{
- q[0] = x - j->node[1].body->posr.pos[0];
- q[1] = y - j->node[1].body->posr.pos[1];
- q[2] = z - j->node[1].body->posr.pos[2];
+ q[0] = x - b1->posr.pos[0];
+ q[1] = y - b1->posr.pos[1];
+ q[2] = z - b1->posr.pos[2];
q[3] = 0;
- dMultiply1_331( anchor2, j->node[1].body->posr.R, q );
+ dMultiply1_331( anchor2, b1->posr.R, q );
}
else
{
@@ -313,7 +332,8 @@
void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
dVector3 axis1, dVector3 axis2 )
{
- if ( j->node[0].body )
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
{
dReal q[4];
q[0] = x;
@@ -321,16 +341,19 @@
q[2] = z;
q[3] = 0;
dNormalize3( q );
+
if ( axis1 )
{
- dMultiply1_331( axis1, j->node[0].body->posr.R, q );
+ dMultiply1_331( axis1, b0->posr.R, q );
axis1[3] = 0;
}
+
if ( axis2 )
{
- if ( j->node[1].body )
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
{
- dMultiply1_331( axis2, j->node[1].body->posr.R, q );
+ dMultiply1_331( axis2, b1->posr.R, q );
}
else
{
@@ -346,24 +369,26 @@
void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
{
- if ( j->node[0].body )
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
{
- dMultiply0_331( result, j->node[0].body->posr.R, anchor1 );
- result[0] += j->node[0].body->posr.pos[0];
- result[1] += j->node[0].body->posr.pos[1];
- result[2] += j->node[0].body->posr.pos[2];
+ dMultiply0_331( result, b0->posr.R, anchor1 );
+ result[0] += b0->posr.pos[0];
+ result[1] += b0->posr.pos[1];
+ result[2] += b0->posr.pos[2];
}
}
void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 )
{
- if ( j->node[1].body )
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
{
- dMultiply0_331( result, j->node[1].body->posr.R, anchor2 );
- result[0] += j->node[1].body->posr.pos[0];
- result[1] += j->node[1].body->posr.pos[1];
- result[2] += j->node[1].body->posr.pos[2];
+ dMultiply0_331( result, b1->posr.R, anchor2 );
+ result[0] += b1->posr.pos[0];
+ result[1] += b1->posr.pos[1];
+ result[2] += b1->posr.pos[2];
}
else
{
@@ -376,18 +401,20 @@
void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
{
- if ( j->node[0].body )
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
{
- dMultiply0_331( result, j->node[0].body->posr.R, axis1 );
+ dMultiply0_331( result, b0->posr.R, axis1 );
}
}
void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 )
{
- if ( j->node[1].body )
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
{
- dMultiply0_331( result, j->node[1].body->posr.R, axis2 );
+ dMultiply0_331( result, b1->posr.R, axis2 );
}
else
{
@@ -589,7 +616,9 @@
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
- if ( joint->node[1].body )
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
@@ -610,12 +639,13 @@
// only need to do this if the constraint connects two bodies.
dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
- if ( !rotational && joint->node[1].body )
+ if ( !rotational && b1 )
{
+ dxBody *b0 = joint->node[0].body;
dVector3 c;
- c[0] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] );
- c[1] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] );
- c[2] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] );
+ c[0] = REAL( 0.5 ) * ( b1->posr.pos[0] - b0->posr.pos[0] );
+ c[1] = REAL( 0.5 ) * ( b1->posr.pos[1] - b0->posr.pos[1] );
+ c[2] = REAL( 0.5 ) * ( b1->posr.pos[2] - b0->posr.pos[2] );
dCalcVectorCross3( ltd, c, ax1 );
info->J1a[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|