void cPhysicsObject::Update(durationms_t currentTime)
{
if (bDynamic) {
const dReal* p0 = nullptr;
const dReal* r0 = nullptr;
dQuaternion q;
if (bBody) {
p0 = dBodyGetPosition(body);
r0 = dBodyGetQuaternion(body);
const dReal* v0 = dBodyGetLinearVel(body);
//const dReal *a0=dBodyGetAngularVel(body);
v[0] = v0[0];
v[1] = v0[1];
v[2] = v0[2];
} else {
p0 = dGeomGetPosition(geom);
dGeomGetQuaternion(geom, q);
r0 = q;
// These are static for the moment
v[0] = 0.0f;
v[1] = 0.0f;
v[2] = 0.0f;
}
ASSERT(p0 != nullptr);
ASSERT(r0 != nullptr);
position.Set(p0[0], p0[1], p0[2]);
rotation.SetFromODEQuaternion(r0);
}
}
Matrix PhysicsGeom::getTransformation(void) const
{
Matrix Transformation;
Vec3f Translation;
const dReal* t = dGeomGetOffsetPosition(_GeomID);
Translation.setValues( t[0],t[1],t[2] );
dQuaternion q;
dGeomGetOffsetQuaternion(_GeomID, q);
Quaternion Rotation;
Rotation.setValueAsQuat(q[1], q[2], q[3], q[0]);
Transformation.setTransform(Translation,Rotation);
if(isPlaceable())
{
t = dGeomGetPosition(_GeomID);
Translation.setValues( t[0],t[1],t[2] );
dGeomGetQuaternion(_GeomID, q);
Rotation.setValueAsQuat(q[1], q[2], q[3], q[0]);
Matrix NonBodyTransformation;
NonBodyTransformation.setTransform(Translation,Rotation);
Transformation.mult(NonBodyTransformation);
}
return Transformation;
}
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);
}
void collidable_object::get_orientation(double& x, double& y, double& z, double& w) const
{
if(body_id)
{
object::get_orientation(x, y, z, w);
return;
}
if(!geom_id) return;
dQuaternion quat; dGeomGetQuaternion (geom_id, quat);
x = quat[1]; y = quat[2]; z = quat[3]; w = quat[0];
}
static void ccdGeomToObj(const dGeomID g, ccd_obj_t *o)
{
const dReal *ode_pos;
dQuaternion ode_rot;
ode_pos = dGeomGetPosition(g);
dGeomGetQuaternion(g, ode_rot);
ccdVec3Set(&o->pos, ode_pos[0], ode_pos[1], ode_pos[2]);
ccdQuatSet(&o->rot, ode_rot[1], ode_rot[2], ode_rot[3], ode_rot[0]);
ccdQuatInvert2(&o->rot_inv, &o->rot);
}
static void printGeomTransform (PrintingContext &c, dxGeom *g)
{
dxGeom *g2 = dGeomTransformGetGeom (g);
const dReal *pos = dGeomGetPosition (g2);
dQuaternion q;
dGeomGetQuaternion (g2,q);
c.print ("type","transform");
c.print ("pos",pos);
c.print ("q",q,4);
c.print ("geometry = {");
c.indent++;
printGeom (c,g2);
c.indent--;
c.print ("}");
}
void PObject::getQuat(Quatf_t quat)
{
dReal dquat[4];
if (isPlaceable())
{
dGeomGetQuaternion(geom, dquat);
DQuatToQuatf(dquat, quat);
}
else
{
quat[0] = 0;
quat[1] = 0;
quat[2] = 0;
quat[3] = 1;
}
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,0);
}
}
const osg::Quat BoxObstacle::getAttitude() {
dQuaternion boxQuat;
dGeomGetQuaternion(boxGeom_, boxQuat);
return osg::Quat(boxQuat[1], boxQuat[2], boxQuat[3], boxQuat[0]);
}
