//! This function is called once per simulation step, allowing the
//! constraint to be "motorized" according to some response.
void OscHinge2ODE::simulationCallback()
{
ODEConstraint& me = *static_cast<ODEConstraint*>(special());
dReal angle1 = dJointGetHinge2Angle1(me.joint());
dReal rate1 = dJointGetHinge2Angle1Rate(me.joint());
dReal addtorque1 =
- m_response->m_stiffness.m_value*angle1
- m_response->m_damping.m_value*rate1;
#if 0 // TODO: dJointGetHinge2Angle2 is not yet available in ODE.
dReal angle2 = dJointGetHinge2Angle2(me.joint());
#else
dReal angle2 = 0;
#endif
dReal rate2 = dJointGetHinge2Angle2Rate(me.joint());
dReal addtorque2 =
- m_response->m_stiffness.m_value*angle2
- m_response->m_damping.m_value*rate2;
m_torque1.m_value = addtorque1;
m_torque2.m_value = addtorque2;
dJointAddHinge2Torques(me.joint(), addtorque1, addtorque2);
}
static float get_phys_joint_rate(dJointID j, int n)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: return (float) DEG(dJointGetHingeAngleRate (j));
case dJointTypeSlider: return (float) dJointGetSliderPositionRate(j);
case dJointTypeHinge2:
if (n == 1)
return (float) DEG(dJointGetHinge2Angle1Rate(j));
else
return (float) DEG(dJointGetHinge2Angle2Rate(j));
default: return 0.0f;
}
}
dReal doStuffAndGetError (int n)
{
switch (n) {
// ********** fixed joint
case 0: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the orientations are the same
const dReal *R1 = dBodyGetRotation (body[0]);
const dReal *R2 = dBodyGetRotation (body[1]);
dReal err1 = dMaxDifference (R1,R2,3,3);
// check the body offset is correct
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return (err1 + length (pp)) * 300;
}
case 1: { // 1 body to static env
addOscillatingTorque (0.1);
// check the orientation is the identity
dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return (err1 + length (p)) * 1e6;
}
case 2: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the body offset is correct
// Should really check body rotation too. Oh well.
const dReal *R1 = dBodyGetRotation (body[0]);
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return length(pp) * 300;
}
case 3: { // 1 body to static env with relative rotation
addOscillatingTorque (0.1);
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return length (p) * 1e6;
}
// ********** hinge joint
case 200: // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
return dInfinity;
case 220: // hinge angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHingeAngle (joint);
if (a > 0.5 && a < 1) return 0; else return 10;
}
return 0;
case 221: { // hinge angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHingeAngle (joint);
dReal r = dJointGetHingeAngleRate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er) * 4e4;
}
case 230: // hinge motor rate (and polarity) test
case 231: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHingeAngleRate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHingeParam (joint,dParamVel,cos(a));
if (n==231) return dInfinity;
return err * 1e6;
}
// ********** slider joint
case 300: // 2 body
addOscillatingTorque (0.05);
dampRotationalMotion (0.1);
addSpringForce (0.5);
return dInfinity;
case 320: // slider angle polarity test
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
if (iteration == 40) {
dReal a = dJointGetSliderPosition (joint);
if (a > 0.2 && a < 0.5) return 0; else return 10;
return a;
}
return 0;
case 321: { // slider angle rate test
static dReal last_pos = 0;
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
dReal p = dJointGetSliderPosition (joint);
dReal r = dJointGetSliderPositionRate (joint);
dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact)
last_pos = p;
return fabs(r-er) * 1e9;
}
case 330: // slider motor rate (and polarity) test
case 331: { // ...with stops
static dReal a = 0;
dReal r = dJointGetSliderPositionRate (joint);
dReal err = fabs (0.7*cos(a) - r);
if (a < 0.04) err = 0;
a += 0.03;
dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
if (n==331) return dInfinity;
return err * 1e6;
}
// ********** hinge-2 joint
case 420: // hinge-2 steering angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHinge2Angle1 (joint);
if (a > 0.5 && a < 0.6) return 0; else return 10;
}
return 0;
case 421: { // hinge-2 steering angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHinge2Angle1 (joint);
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er)*2e4;
}
case 430: // hinge 2 steering motor rate (+polarity) test
case 431: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel,cos(a));
if (n==431) return dInfinity;
return err * 1e6;
}
case 432: { // hinge 2 wheel motor rate (+polarity) test
static dReal a = 0;
dReal r = dJointGetHinge2Angle2Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel2,cos(a));
return err * 1e6;
}
// ********** angular motor joint
case 600: { // test euler angle calculations
// desired euler angles from last iteration
static dReal a1,a2,a3;
// find actual euler angles
dReal aa1 = dJointGetAMotorAngle (joint,0);
dReal aa2 = dJointGetAMotorAngle (joint,1);
dReal aa3 = dJointGetAMotorAngle (joint,2);
// printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);
dReal err = dInfinity;
if (iteration > 0) {
err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
err *= 1e10;
}
// get random base rotation for both bodies
dMatrix3 Rbase;
dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
dBodySetRotation (body[0],Rbase);
// rotate body 2 by random euler angles w.r.t. body 1
a1 = 3.14 * 2 * (dRandReal()-0.5);
a2 = 1.57 * 2 * (dRandReal()-0.5);
a3 = 3.14 * 2 * (dRandReal()-0.5);
dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
dRFromAxisAndAngle (R1,0,0,1,-a1);
dRFromAxisAndAngle (R2,0,1,0,a2);
dRFromAxisAndAngle (R3,1,0,0,-a3);
dMultiply0 (Rtmp1,R2,R3,3,3,3);
dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
dBodySetRotation (body[1],Rtmp1);
// printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);
return err;
}
// ********** universal joint
case 700: { // 2 body: joint constraint
dVector3 ax1, ax2;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 701: { // 2 body: angle 1 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 702: { // 2 body: angle 2 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 720: { // universal transmit torque test: constraint error
dVector3 ax1, ax2;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 721: { // universal transmit torque test: angle1 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 722: { // universal transmit torque test: angle2 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 730:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 731:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 2e3;
}
case 732:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 740:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 741:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 742:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e4;
}
}
return dInfinity;
}
float CCar::SWheelDrive::ASpeed()
{
CPhysicsJoint* J=pwheel->joint;
if(!J) return 0.f;
return (dJointGetHinge2Angle2Rate(J->GetDJoint()))*pos_fvd;//dFabs
}
//when two geoms might intersect
void Geom::Collision_Callback (void *data, dGeomID o1, dGeomID o2)
{
//check if one (or both) geom is space
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
dSpaceCollide2 (o1,o2,data, &Collision_Callback);
return;
}
//both geoms are geoms, get component_data from geoms
Geom *geom1, *geom2;
geom1 = (Geom*) dGeomGetData (o1);
geom2 = (Geom*) dGeomGetData (o2);
//get attached bodies
dBodyID b1, b2;
b1 = dGeomGetBody(o1);
b2 = dGeomGetBody(o2);
//none connected to bodies
if (!b1 && !b2)
return;
//none wants to create collisions..
if (!geom1->collide&&!geom2->collide)
{
printlog(1, "not collideable, TODO: bitfield solution");
return;
}
dContact contact[internal.contact_points];
int count = dCollide (o1,o2,internal.contact_points, &contact[0].geom, sizeof(dContact));
if (count == 0)
return;
//does both components want to collide for real? (not "ghosts"/"sensors")
if (geom1->collide&&geom2->collide)
{
int mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
dReal slip,mu,erp,cfm;
dReal bounce = 0;
dVector3 fdir = {0,0,0};
mu = (geom1->mu)*(geom2->mu);
erp = (geom1->erp)*(geom2->erp);
cfm = (geom1->cfm)*(geom2->cfm);
slip = 0.0;
bool feedback = false;
//if any of the geoms responds to forces or got a body that responds to force, enable force feedback
if (geom1->buffer_event || geom2->buffer_event || geom1->force_to_body || geom2->force_to_body)
feedback = true;
//optional bouncyness (good for wheels?)
if (geom1->bounce||geom2->bounce)
{
mode |= dContactBounce;
bounce = (geom1->bounce)*(geom2->bounce);
}
//determine if _one_of the geoms is a wheel
Geom *other, *wheel = NULL;
if (geom1->wheel&&!geom2->wheel)
{
wheel = geom1;
other = geom2;
}
else if (!geom1->wheel&&geom2->wheel)
{
wheel = geom2;
other = geom1;
}
int i;
if (wheel)
{
int mode_tyre = mode | dContactSlip1 | dContactFDir1; //add slip calculations and specified direction
//get slip value (based on the two geoms' slip value and the wheel's rotation speed)
dReal speed = dJointGetHinge2Angle2Rate (wheel->hinge2);
if (speed < 0)
speed = -speed;
slip = (geom1->slip)*(geom2->slip)*speed;
//now get the axis direction of the wheel (for slip and rim detection), note: axis is along Z
const dReal *rot = dGeomGetRotation(wheel->geom_id);
fdir[0] = rot[2];
fdir[1] = rot[6];
fdir[2] = rot[10];
//when rim is colliding, no slip, different mu...
dReal mu_rim = (wheel->mu_rim)*(other->mu);
//note: there's gotta be another way instead of storing a mu_rim in every Geom...
for (i=0; i<count; ++i)
{
//dot product between wheel axis and force direction (contact normal)
dReal dot = contact[i].geom.normal[0]*fdir[0]+contact[i].geom.normal[1]*fdir[1]+contact[i].geom.normal[2]*fdir[2];
//tyre
if (-internal.rim_angle < dot && dot < internal.rim_angle)
{
contact[i].surface.mode = mode_tyre;
contact[i].fdir1[0] = fdir[0];
contact[i].fdir1[1] = fdir[1];
contact[i].fdir1[2] = fdir[2];
contact[i].surface.slip1 = slip;
contact[i].surface.mu = mu;
contact[i].surface.soft_erp = erp;
contact[i].surface.soft_cfm = cfm;
contact[i].surface.bounce = bounce; //in case specified
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,b1,b2);
if (feedback)
new Collision_Feedback(c, geom1, geom2);
}
//rim
else
{
contact[i].surface.mode = mode;
contact[i].surface.mu = mu_rim;
contact[i].surface.soft_erp = erp;
contact[i].surface.soft_cfm = cfm;
contact[i].surface.bounce = bounce; //in case specified
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,b1,b2);
if (feedback)
new Collision_Feedback(c, geom1, geom2);
}
}
}
else //normal collision
{
for (i=0; i<count; ++i)
{
contact[i].surface.mode = mode;
contact[i].surface.mu = mu;
contact[i].surface.soft_erp = erp;
contact[i].surface.soft_cfm = cfm;
contact[i].surface.bounce = bounce; //in case specified
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,b1,b2);
if (feedback)
new Collision_Feedback(c, geom1, geom2);
}
}
}
//with physical contact or not, might respond to collision events
if (geom1->collide)
{
geom2->colliding = true;
}
if (geom2->collide)
{
geom1->colliding = true;
}
}
Real32 PhysicsHinge2Joint::getAngle2Rate(void)
{
PhysicsHinge2JointPtr tmpPtr(*this);
return dJointGetHinge2Angle2Rate(tmpPtr->id);
}
