Menu
▾
▴
[Opende-svn] SF.net SVN: opende:[1638] branches/testRotationalLimits
|
From: <pa...@us...> - 2009-01-29 17:04:59
|
Revision: 1638
http://opende.svn.sourceforge.net/opende/?rev=1638&view=rev
Author: papaDoc
Date: 2009-01-29 17:04:54 +0000 (Thu, 29 Jan 2009)
Log Message:
-----------
* Fix bug: Fix problem when attaching no body to a joint. Before calling
setRelativeValues a check is made for bodies.
* Add an epsilon to the testRotationalLimit to avoid round off error
or numerical error when the diff is not in the same direction as the rate.
* Remove compilation warning
* Add unittest
Modified Paths:
--------------
branches/testRotationalLimits/CHANGELOG.txt
branches/testRotationalLimits/ode/src/joints/joint.cpp
branches/testRotationalLimits/ode/src/ode.cpp
branches/testRotationalLimits/tests/joint.cpp
branches/testRotationalLimits/tests/joints/ball.cpp
branches/testRotationalLimits/tests/joints/hinge.cpp
branches/testRotationalLimits/tests/joints/hinge2.cpp
branches/testRotationalLimits/tests/joints/universal.cpp
Modified: branches/testRotationalLimits/CHANGELOG.txt
===================================================================
--- branches/testRotationalLimits/CHANGELOG.txt 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/CHANGELOG.txt 2009-01-29 17:04:54 UTC (rev 1638)
@@ -9,6 +9,14 @@
------------------------------------------------------------------------------
+01/29/09 Remi Ricard (papaDoc)
+ * Fix bug: Fix problem when attaching no body to a joint. Before calling
+ setRelativeValues a check is made for bodies.
+ * Add an epsilon to the testRotationalLimit to avoid round off error
+ or numerical error when the diff is not in the same direction as the rate.
+ * Remove compilation warning
+ * Add unittest
+
01/27/09 Remi Ricard (papaDoc)
* Add an epsilon to the testRotationalLimit to avoid round off error
or numerical error.
Modified: branches/testRotationalLimits/ode/src/joints/joint.cpp
===================================================================
--- branches/testRotationalLimits/ode/src/joints/joint.cpp 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/ode/src/joints/joint.cpp 2009-01-29 17:04:54 UTC (rev 1638)
@@ -569,7 +569,7 @@
{
dReal diff = *angle - prevAngle;
// The angle value must have increase
- if ( diff < 0 )
+ if ( diff < -(REAL(0.25)*M_PI) )
{
// A wrap around occured, undo it to get the good limit crossing
*angle += 2*M_PI;
@@ -590,7 +590,7 @@
dReal diff = *angle - prevAngle;
// The angle value must have decrease
- if ( diff > 0 )
+ if ( diff > (REAL(0.25)*M_PI) )
{
// A wrap around occured, undo it to get the good limit crossing
*angle -= 2*M_PI;
Modified: branches/testRotationalLimits/ode/src/ode.cpp
===================================================================
--- branches/testRotationalLimits/ode/src/ode.cpp 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/ode/src/ode.cpp 2009-01-29 17:04:54 UTC (rev 1638)
@@ -1380,7 +1380,10 @@
joint->node[1].next = body1->firstjoint;
body1->firstjoint = &joint->node[1];
}
- else joint->node[1].next = 0;
+ else {
+ joint->node[1].next = 0;
+ }
+
if (body2) {
joint->node[0].next = body2->firstjoint;
body2->firstjoint = &joint->node[0];
@@ -1390,8 +1393,10 @@
}
// Since the bodies are now set.
- // Calculate the values depending on the bodies
- joint->setRelativeValues();
+ // Calculate the values depending on the bodies.
+ // Only need to calculate relative value if a body exist
+ if (body1 || body2)
+ joint->setRelativeValues();
}
void dJointEnable (dxJoint *joint)
Modified: branches/testRotationalLimits/tests/joint.cpp
===================================================================
--- branches/testRotationalLimits/tests/joint.cpp 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/tests/joint.cpp 2009-01-29 17:04:54 UTC (rev 1638)
@@ -2975,4 +2975,73 @@
CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );
}
+
+
+ struct Fixture_Simple_Hinge
+ {
+ Fixture_Simple_Hinge ()
+ {
+ wId = dWorldCreate();
+
+ bId1 = dBodyCreate(wId);
+ dBodySetPosition(bId1, 0, -1, 0);
+
+ bId2 = dBodyCreate(wId);
+ dBodySetPosition(bId2, 0, 1, 0);
+
+
+ jId = dJointCreateHinge(wId, 0);
+
+ dJointAttach(jId, bId1, bId2);
+ }
+
+ ~Fixture_Simple_Hinge()
+ {
+ dWorldDestroy(wId);
+ }
+
+ dJointID jId;
+
+ dWorldID wId;
+
+ dBodyID bId1;
+ dBodyID bId2;
+ };
+
+ // Test that it is possible to have joint without a body
+ TEST_FIXTURE(Fixture_Simple_Hinge, test_dJointAttach)
+ {
+ bool only_body1_OK = true;
+ try {
+ dJointAttach(jId, bId1, 0);
+ dWorldStep (wId, 1);
+ }
+ catch (...) {
+ only_body1_OK = false;
+ }
+ CHECK_EQUAL(true, only_body1_OK);
+
+ bool only_body2_OK = true;
+ try {
+ dJointAttach(jId, 0, bId2);
+ dWorldStep (wId, 1);
+ }
+ catch (...) {
+ only_body2_OK = false;
+ }
+ CHECK_EQUAL(true, only_body2_OK);
+
+ bool no_body_OK = true;
+ try {
+ dJointAttach(jId, 0, 0);
+ dWorldStep (wId, 1);
+ }
+ catch (...) {
+ no_body_OK = false;
+ }
+ CHECK_EQUAL(true, no_body_OK);
+ }
+
+
+
} // End of SUITE(JointPiston)
Modified: branches/testRotationalLimits/tests/joints/ball.cpp
===================================================================
--- branches/testRotationalLimits/tests/joints/ball.cpp 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/tests/joints/ball.cpp 2009-01-29 17:04:54 UTC (rev 1638)
@@ -60,18 +60,18 @@
dMatrix3 R;
dVector3 axis; // Random axis
- axis[0] = 0.53;
- axis[1] = -0.71;
- axis[2] = 0.43;
+ axis[0] = REAL(0.53);
+ axis[1] = -REAL(0.71);
+ axis[2] = REAL(0.43);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
dBodySetRotation (bId[j][0], R);
- axis[0] = 1.2;
- axis[1] = 0.87;
- axis[2] = -0.33;
+ axis[0] = REAL(1.2);
+ axis[1] = REAL(0.87);
+ axis[2] = -REAL(0.33);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
Modified: branches/testRotationalLimits/tests/joints/hinge.cpp
===================================================================
--- branches/testRotationalLimits/tests/joints/hinge.cpp 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/tests/joints/hinge.cpp 2009-01-29 17:04:54 UTC (rev 1638)
@@ -1301,18 +1301,18 @@
bId1 = dBodyCreate (wId);
dBodySetMass (bId1,&m);
- dBodySetPosition (bId1, 0.5*SIDE, 0.5*SIDE, 1);
+ dBodySetPosition (bId1, REAL(0.5)*SIDE, REAL(0.5)*SIDE, 1);
dBodySetQuaternion (bId1,q);
bId2 = dBodyCreate (wId);
dBodySetMass (bId2,&m);
- dBodySetPosition (bId2,-0.5*SIDE,-0.5*SIDE,1);
+ dBodySetPosition (bId2,-REAL(0.5)*SIDE,-REAL(0.5)*SIDE,1);
dBodySetQuaternion (bId2,q);
jId = dJointCreateHinge (wId, 0);
dJointAttach (jId, bId1, bId2);
dJointSetHingeAnchor (jId, 0, 0, 1);
- dJointSetHingeAxis (jId, 1, -1, 1.41421356);
+ dJointSetHingeAxis (jId, 1, -1, REAL(1.41421356));
}
~dxJointHinge_Fixture_as_demo_hinge()
@@ -1320,15 +1320,18 @@
dWorldDestroy (wId);
}
+ void
simLoop()
{
- const dReal kd = -0.3; // angular damping constant
+ const dReal kd = -REAL(0.3); // angular damping constant
static dReal a=0;
const dReal *w = dBodyGetAngularVel (bId1);
- dBodyAddTorque (bId1,kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
+ dBodyAddTorque (bId1, kd*w[0],
+ kd*w[1] + REAL(0.1)*cos(a),
+ kd*w[2] + REAL(0.1)*sin(a));
dWorldStep (wId, 0.05);
- a += 0.01;
+ a += REAL(0.01);
}
@@ -1355,13 +1358,11 @@
CHECK_CLOSE (0, r2d(dJointGetHingeAngle(jId)), 1e-4);
- dxJointHinge_Fixture_as_demo_hinge::simLoop();
-
- CHECK_CLOSE (0, r2d(dJointGetHingeAngle (jId)), 1e-4);
-
- dxJointHinge_Fixture_as_demo_hinge::simLoop();
-
- CHECK_CLOSE (0, r2d(dJointGetHingeAngle (jId)), 1e-4);
+ for (int i=0; i < 300; ++i)
+ {
+ dxJointHinge_Fixture_as_demo_hinge::simLoop();
+ CHECK_CLOSE (0, r2d(dJointGetHingeAngle (jId)), 1e-4);
+ }
}
Modified: branches/testRotationalLimits/tests/joints/hinge2.cpp
===================================================================
--- branches/testRotationalLimits/tests/joints/hinge2.cpp 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/tests/joints/hinge2.cpp 2009-01-29 17:04:54 UTC (rev 1638)
@@ -60,18 +60,18 @@
dMatrix3 R;
dVector3 axis; // Random axis
- axis[0] = 0.53;
- axis[1] = -0.71;
- axis[2] = 0.43;
+ axis[0] = REAL(0.53);
+ axis[1] = -REAL(0.71);
+ axis[2] = REAL(0.43);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
dBodySetRotation (bId[j][0], R);
- axis[0] = 1.2;
- axis[1] = 0.87;
- axis[2] = -0.33;
+ axis[0] = REAL(1.2);
+ axis[1] = REAL(0.87);
+ axis[2] = -REAL(0.33);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
Modified: branches/testRotationalLimits/tests/joints/universal.cpp
===================================================================
--- branches/testRotationalLimits/tests/joints/universal.cpp 2009-01-29 16:28:55 UTC (rev 1637)
+++ branches/testRotationalLimits/tests/joints/universal.cpp 2009-01-29 17:04:54 UTC (rev 1638)
@@ -109,18 +109,18 @@
dVector3 axis;
- axis[0] = 0.53;
- axis[1] = -0.71;
- axis[2] = 0.43;
+ axis[0] = REAL(0.53);
+ axis[1] = -REAL(0.71);
+ axis[2] = REAL(0.43);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
dBodySetRotation (bId1, R);
- axis[0] = 1.2;
- axis[1] = 0.87;
- axis[2] = -0.33;
+ axis[0] = REAL(1.2);
+ axis[1] = REAL(0.87);
+ axis[2] = -REAL(0.33);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
@@ -280,15 +280,15 @@
// ax1 and ax2 are pseudo-random axis. N.B. They are NOT the axis of the joints.
dVector3 ax1;
- ax1[0] = 0.2;
- ax1[1] = -0.67;
- ax1[2] = -0.81;
+ ax1[0] = REAL(0.2);
+ ax1[1] = -REAL(0.67);
+ ax1[2] = -REAL(0.81);
dNormalize3(ax1);
dVector3 ax2;
- ax2[0] = 0.62;
- ax2[1] = 0.31;
- ax2[2] = 0.43;
+ ax2[0] = REAL(0.62);
+ ax2[1] = REAL(0.31);
+ ax2[2] = REAL(0.43);
dNormalize3(ax2);
@@ -333,14 +333,14 @@
dBodySetRotation (bId1, R);
dBodySetRotation (bId1, R);
- axis1[0] = 0.32;
- axis1[1] = -0.57;
- axis1[2] = 0.71;
+ axis1[0] = REAL(0.32);
+ axis1[1] = -REAL(0.57);
+ axis1[2] = REAL(0.71);
dNormalize3(axis1);
- axis2[0] = 0.-26;
- axis2[1] = -0.31;
- axis2[2] = 0.69;
+ axis2[0] = -REAL(0.26);
+ axis2[1] = -REAL(0.31);
+ axis2[2] = REAL(0.69);
dNormalize3(axis2);
dVector3 cross;
@@ -627,18 +627,18 @@
dMatrix3 R;
dVector3 axis; // Random axis
- axis[0] = 0.53;
- axis[1] = -0.71;
- axis[2] = 0.43;
+ axis[0] = REAL(0.53);
+ axis[1] = -REAL(0.71);
+ axis[2] = REAL(0.43);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
dBodySetRotation (bId[j][0], R);
- axis[0] = 1.2;
- axis[1] = 0.87;
- axis[2] = -0.33;
+ axis[0] = REAL(1.2);
+ axis[1] = REAL(0.87);
+ axis[2] = -REAL(0.33);
dNormalize3(axis);
dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
REAL(0.47123)); // 27deg
@@ -2020,15 +2020,15 @@
dJointAttach (jId, bId1, bId2);
dVector3 axis1;
- axis1[0] = 0.53;
- axis1[1] = -0.71;
- axis1[2] = 0.43;
+ axis1[0] = REAL(0.53);
+ axis1[1] = -REAL(0.71);
+ axis1[2] = REAL(0.43);
dNormalize3(axis1);
dVector3 axis;
- axis[0] = 1.2;
- axis[1] = 0.87;
- axis[2] = -0.33;
+ axis[0] = REAL(1.2);
+ axis[1] = REAL(0.87);
+ axis[2] = -REAL(0.33);
dVector3 axis2;
dCROSS(axis2, =, axis1, axis);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|