void drawGeom( dGeomID g, int colored = 0 )
{
if( !g ) //If the geometry object is missing, end the function.
return;
const dReal *position; //Define pointers to internal positions and orientations.
const dReal *orientation; //Pointers to constant objects (so the objects will not change).
int type = dGeomGetClass( g ); //Get the type of geometry.
position = dGeomGetPosition( g ); //Then, get the geometry position.
orientation = dGeomGetRotation( g ); //And get existing geometry orientation.
if( type == dBoxClass ) //Is it a box?
{
dReal sides[3];
dGeomBoxGetLengths( g, sides ); //Get length of sides.
renderBox( sides, position, orientation, colored ); //Render the actual box in environment.
}
else if (type == dCylinderClass)
{
dReal radius, length;
dGeomCylinderGetParams(g, &radius, &length);
renderCylinder(radius, length, position, orientation);
}
else if (type == dCapsuleClass)
{
dReal radius, length;
dGeomCapsuleGetParams(g, &radius, &length);
renderCapsule(radius, length, position, orientation);
}
}
static void printCylinder (PrintingContext &c, dxGeom *g)
{
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
c.print ("type","cylinder");
c.print ("radius",radius);
c.print ("length",length);
}
void TSRODERigidBody::DebugRender()
{
Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState );
TSRMatrix4 bodyTransform;
TSRMatrix4 geomTransform;
const float* pBodyPosition = dBodyGetPosition( m_BodyID );
const float* pBodyRotation = dBodyGetRotation( m_BodyID );
ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
TSRDebugDraw::RenderAxis( 1.0f );
TSRDebugDraw::RenderSphere( 0.25f );
for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
{
dGeomID currGeomTransformID = m_GeomIDs[ i ];
dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID );
const float* pGeomPosition = dGeomGetPosition( geomID );
const float* pGeomRotation = dGeomGetRotation( geomID );
ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
switch( dGeomGetClass( geomID ) )
{
case dBoxClass:
{
dVector3 extents;
dGeomBoxGetLengths( geomID, extents );
TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] );
TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] );
aabbMin *= 0.5f;
aabbMax *= 0.5f;
TSRDebugDraw::RenderAABB( aabbMin, aabbMax );
}
break;
case dSphereClass:
{
float radius;
radius = dGeomSphereGetRadius( geomID );
TSRDebugDraw::RenderSphere( radius );
}
break;
case dCylinderClass:
{
float radius,length;
dGeomCylinderGetParams( geomID, &radius, &length );
TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) );
}
break;
}
TSRGlobalConstants.PopMatrix();
}
TSRGlobalConstants.PopMatrix();
Graphics()->SetRasterizerState( Graphics()->m_FillSolidState );
}
static void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *cyl)
{
dReal r, h;
ccdGeomToObj(g, (ccd_obj_t *)cyl);
dGeomCylinderGetParams(g, &r, &h);
cyl->radius = r;
cyl->height = h / 2.;
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMultiply0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMultiply0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
/*** 車輪の描画 ***/
void drawWheel()
{
dReal radius, length;
dsSetColor(1.1,1.1,1.1);
for (int i=0; i< WHEEL_NUM; i++)
{
dGeomCylinderGetParams(wheel[i].geom, &radius, &length);
dsDrawCylinder(dGeomGetPosition(wheel[i].geom),
dGeomGetRotation(wheel[i].geom),length, radius);
}
}
void CManipulator::drawCylinder (dGeomID id, float R, float G, float B)
{
if (!id)
return;
const dReal *pos = dGeomGetPosition (id);
const dReal *rot = dGeomGetRotation (id);
dsSetColor (R,G,B);
dReal l,r;
dGeomCylinderGetParams(id,&r,&l);
dsDrawCylinderD(pos, rot, (float)l,(float)r);
}
void DrawOmni(){
/* 車輪の描画 */
dReal radius, length;
dsSetColor(0.3, 0.3, 0.3);
for (int i=0; i<OMNI_NUM; i++)
{
for (int j=0; j< WHEEL_NUM; j++)
{
dGeomCylinderGetParams(wheel[i][j].geom, &radius, &length);
dsDrawCylinder(dGeomGetPosition(wheel[i][j].geom), dGeomGetRotation(wheel[i][j].geom),length, radius);
}
/* 土台の描画 */
dsSetColor(0.3, 0.1, 0.1);
dsDrawBox(dBodyGetPosition(base[i].body),dBodyGetRotation(base[i].body),baseSize[i]);
}
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
} else if (type == dConvexClass) {
//dVector3 sides={0.50,0.50,0.50};
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void GetCylinderExtensions(dGeomID cyl,const dReal* axis,
const dReal *pos, const dReal *rot,
float center_prg,dReal* lo_ext,dReal* hi_ext)
{
R_ASSERT2(dGeomGetClass(cyl)==dCylinderClassUser,"is not a cylinder");
dReal radius,length;
dGeomCylinderGetParams(cyl,&radius,&length);
dReal dif=dDOT(pos,axis)-center_prg;
dReal _cos=dFabs(dDOT14(axis,rot+1));
dReal cos1=dDOT14(axis,rot+0);
dReal cos3=dDOT14(axis,rot+2);
dReal _sin=_sqrt(cos1*cos1+cos3*cos3);
length/=2.f;
dReal ful_ext=_cos*length+_sin*radius;
*lo_ext=-ful_ext+dif;
*hi_ext=ful_ext+dif;
}
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 dmDraw(dmObject *obj) /*** 物体の描画 ***/
{
if (!obj->geom) return;
obj->p = (double *) dGeomGetPosition(obj->geom);
obj->R = (double *) dGeomGetRotation(obj->geom);
dsSetColor(obj->color[0],obj->color[1],obj->color[2]); // 色の設定(r,g,b)
// printf("color=%.1f %.1f %.1f\n",obj->color[0],obj->color[1],obj->color[2]);
int type = dGeomGetClass(obj->geom);
switch (type)
{
case dBoxClass:
{
dVector3 sides;
dGeomBoxGetLengths(obj->geom,sides);
dsDrawBox(obj->p,obj->R,sides);
}
break;
case dSphereClass:
dsDrawSphere(obj->p,obj->R,dGeomSphereGetRadius(obj->geom));
break;
case dCapsuleClass:
{
dReal radius,length;
dGeomCapsuleGetParams(obj->geom,&radius,&length);
dsDrawCapsule(obj->p,obj->R,length,radius);
}
break;
case dCylinderClass:
{
dReal radius,length;
dGeomCylinderGetParams(obj->geom,&radius,&length);
dsDrawCylinder(obj->p,obj->R,length,radius);
}
break;
default:
printf("Bad geometry type \n");
}
}
void drawGeom (dGeomID g)
{
const dReal *pos = dGeomGetPosition(g);
const dReal *R = dGeomGetRotation(g);
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
dVector3 sides;
dGeomBoxGetLengths (g, sides);
dsDrawBox (pos,R,sides);
}
if (type == dCylinderClass)
{
dReal r,l;
dGeomCylinderGetParams(g, &r, &l);
dsDrawCylinder (pos, R, l, r);
}
}
void CManipulator::drawCylinderWithBall (dGeomID idGeom, float R, float G, float B)
{
if (!idGeom)
return;
dVector3 pos1;
const dReal *pos = dGeomGetPosition (idGeom);
const dReal *rot = dGeomGetRotation (idGeom);
dsSetColor (R,G,B);
dReal l,r;
dGeomCylinderGetParams(idGeom,&r,&l);
dsDrawCylinderD(pos, rot, (float)l,(float)r);
pos1[0]=pos[0]+(l/2.0)*rot[2]*-1;
pos1[1]=pos[1]+(l/2.0)*rot[6]*-1;
pos1[2]=pos[2]+(l/2.0)*rot[10]*-1;
dsDrawSphereD(pos1,rot,float(r+BORDER));
pos1[0]=pos[0]+(l/2.0)*rot[2];
pos1[1]=pos[1]+(l/2.0)*rot[6];
pos1[2]=pos[2]+(l/2.0)*rot[10];
dsDrawSphereD(pos1,rot,float(r+BORDER));
}
void SkidSteeringVehicle::draw() {
{
dsSetColor(0, 0, 1);
const dReal *pos = dGeomGetPosition(this->vehicleGeom);
const dReal *R = dGeomGetRotation(this->vehicleGeom);
dReal sides[3];
dGeomBoxGetLengths(this->vehicleGeom, sides);
dsDrawBoxD(pos, R, sides);
}
dsSetColor(1, 1, 0);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
const dReal *pos = dGeomGetPosition(this->wheelGeom[fr][lr]);
const dReal *R = dGeomGetRotation(this->wheelGeom[fr][lr]);
dReal radius, length;
dGeomCylinderGetParams(this->wheelGeom[fr][lr], &radius, &length);
dsDrawCylinderD(pos, R, length, radius);
}
}
}
int dcTriListCollider::CollideCylinder(dxGeom* Cylinder, int Flags, dContactGeom* Contacts, int Stride){
Fvector AABB;
dReal CylinderRadius,CylinderLength;
dGeomCylinderGetParams (Cylinder, &CylinderRadius,&CylinderLength);
dReal* R=const_cast<dReal*>(dGeomGetRotation(Cylinder));
AABB.x = REAL(0.5) * dFabs (R[1] * CylinderLength) + (_sqrt(R[0]*R[0]+R[2]*R[2]) * CylinderRadius);
AABB.y = REAL(0.5) * dFabs (R[5] * CylinderLength) + (_sqrt(R[4]*R[4]+R[6]*R[6]) * CylinderRadius);
AABB.z = REAL(0.5) * dFabs (R[9] * CylinderLength) + (_sqrt(R[8]*R[8]+R[10]*R[10]) * CylinderRadius);
const dReal*velocity=dBodyGetLinearVel(dGeomGetBody(Cylinder));
AABB.x+=dFabs(velocity[0])*0.04f;
AABB.y+=dFabs(velocity[1])*0.04f;
AABB.z+=dFabs(velocity[2])*0.04f;
CylTri ct(*this);
return dSortTriPrimitiveCollide
(
ct,
Cylinder,
Geometry,
Flags,
Contacts,
Stride,
AABB
);
}
//! setting of mass
void SSimRobotEntity::setMass(SSimObjParts *parts, double mass)
{
// if a part is already added
int geomNum = parts->geoms.size();
if (geomNum != 0) {
// mass per each part
double ms = mass / geomNum;
// refer geometry and body
dBodyID body = parts->body;
dMass m;
dMass m2;
dMassSetZero(&m);
dMassSetZero(&m2);
for (int i = 0; i < geomNum; i++) {
// Refer the type of geometry
dGeomID geom = dGeomTransformGetGeom(parts->geoms[i]);
int type = dGeomGetClass(geom);
// setting of mass
// sphere
if (type == 0) {
dReal radius = dGeomSphereGetRadius(geom);
dMassSetSphereTotal(&m2, ms, radius);
}
// box
else if (type == 1) {
dVector3 size;
dGeomBoxGetLengths(geom, size);
dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]);
}
// cylinder
else if (type == 3) {
dReal radius = 0.0;
dReal length = 0.0;
dGeomCylinderGetParams(geom, &radius, &length);
// TODO: confirm: Is 2 suitable for long axis?
dMassSetCylinderTotal(&m2, ms, 2, radius, length);
}
//
const dReal *pos = dGeomGetPosition(geom);
//LOG_MSG(("pos = (%f, %f, %f)", pos[0], pos[1], pos[2]));
dMassTranslate(&m2, pos[0], pos[1], pos[2]);
dMassAdd(&m, &m2);
}
// adjustment of the gap between CoG
//const dReal *p = dBodyGetPosition(body);
//dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]);
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
// seeting of mass
dBodySetMass(body, &m);
dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test
} // if (partsNum != 0) {
}
// 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);
}
}
// initialize collision data
void sCylinderBoxData::_cldInitCylinderBox()
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(m_gCylinder);
dMatrix3Copy(pRotCyc,m_mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(m_gCylinder);
dVector3Copy(*pPosCyc,m_vCylinderPos);
dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(m_gCylinder,&m_fCylinderRadius,&m_fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(m_gBox);
dMatrix3Copy(pRotBox,m_mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(m_gBox);
dVector3Copy(*pPosBox,m_vBoxPos);
dGeomBoxGetLengths(m_gBox, m_vBoxHalfSize);
m_vBoxHalfSize[0] *= REAL(0.5);
m_vBoxHalfSize[1] *= REAL(0.5);
m_vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
m_avBoxVertices[0][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[0][1] = m_vBoxHalfSize[1];
m_avBoxVertices[0][2] = -m_vBoxHalfSize[2];
// vertex 1
m_avBoxVertices[1][0] = m_vBoxHalfSize[0];
m_avBoxVertices[1][1] = m_vBoxHalfSize[1];
m_avBoxVertices[1][2] = -m_vBoxHalfSize[2];
// vertex 2
m_avBoxVertices[2][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[2][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[2][2] = -m_vBoxHalfSize[2];
// vertex 3
m_avBoxVertices[3][0] = m_vBoxHalfSize[0];
m_avBoxVertices[3][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[3][2] = -m_vBoxHalfSize[2];
// vertex 4
m_avBoxVertices[4][0] = m_vBoxHalfSize[0];
m_avBoxVertices[4][1] = m_vBoxHalfSize[1];
m_avBoxVertices[4][2] = m_vBoxHalfSize[2];
// vertex 5
m_avBoxVertices[5][0] = m_vBoxHalfSize[0];
m_avBoxVertices[5][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[5][2] = m_vBoxHalfSize[2];
// vertex 6
m_avBoxVertices[6][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[6][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[6][2] = m_vBoxHalfSize[2];
// vertex 7
m_avBoxVertices[7][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[7][1] = m_vBoxHalfSize[1];
m_avBoxVertices[7][2] = m_vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(m_mBoxRot,m_avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], m_vBoxPos, m_avBoxVertices[i]);
}
// find relative position
dVector3Subtract(m_vCylinderPos,m_vBoxPos,m_vDiff);
m_fBestDepth = MAX_FLOAT;
m_vNormal[0] = REAL(0.0);
m_vNormal[1] = REAL(0.0);
m_vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = (dReal) (M_PI/nCYLINDER_SEGMENT);
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
m_avCylinderNormals[i][0] = -dCos(fAngle);
m_avCylinderNormals[i][1] = -dSin(fAngle);
m_avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
m_fBestrb = 0;
m_fBestrc = 0;
m_iBestAxis = 0;
m_nContacts = 0;
}
static void drawGeom(dGeomID geomID)
{
// printf("1%lu", (uint64_t)geomID);
int gclass = dGeomGetClass(geomID);
// printf("2\n");
const dReal *pos = NULL;
const dReal *rot = NULL;
bool canDrawJoints = false;
ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID);
if (nullptr != userObj) {
dsSetColorAlpha(1, 1, 1, 1);
// printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]);
dsSetTexture (userObj->textureNum);
dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]);
} else {
dsSetColorAlpha(1, 1, 0, 1);
dsSetTexture (DS_WOOD);
}
switch (gclass) {
case dSphereClass:
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID));
}
canDrawJoints = true;
break;
case dBoxClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dVector3 lengths;
dGeomBoxGetLengths(geomID, lengths);
dsDrawBox(pos, rot, lengths);
}
canDrawJoints = true;
break;
}
case dCylinderClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dReal length;
dReal radius;
dGeomCylinderGetParams(geomID, &radius, &length);
dsDrawCylinder(pos, rot, length, radius);
}
canDrawJoints = true;
break;
}
default:
break;
}
// printf("class: %d\n", gclass);
if (canDrawJoints) {
#ifdef DRAW_JOINTS_TOO
dBodyID body = dGeomGetBody(geomID);
int numJoints = dBodyGetNumJoints(body);
for (int i = 0; i < numJoints; ++i)
{
dJointID joint = dBodyGetJoint(body, i);
int jointClass = dJointGetType(joint);
switch (jointClass)
{
case dJointTypeHinge:
{
dVector3 a11;
dBodyID body1 = dJointGetBody(joint, 0);
dBodyID body2 = dJointGetBody(joint, 1);
if (body1 && body2) {
const dReal* bodyPos1 = dBodyGetPosition(body1);
const dReal* bodyPos2 = dBodyGetPosition(body2);
dJointGetHingeAnchor(joint, a11);
dsSetColor(1, 0, 0);
dsDrawLine(a11, bodyPos1);
dsDrawLine(a11, bodyPos2);
dsSetColor(0, 1, 0);
dsDrawLine(bodyPos1, bodyPos2);
}
}
}
}
#endif
}
}
bool BodyVerifier::verify(const RobotRepresentation &robotRep, int &errorCode,
std::vector<std::pair<std::string, std::string> > &affectedBodyParts) {
bool success = true;
errorCode = INTERNAL_ERROR;
// Initialize ODE
dInitODE();
dWorldID odeWorld = dWorldCreate();
dWorldSetGravity(odeWorld, 0, 0, 0);
dSpaceID odeSpace = dHashSpaceCreate(0);
CollisionData *collisionData = new CollisionData();
/* don't want visual debugging
#ifdef VISUAL_DEBUG
// Initialize OSG
osgViewer::Viewer viewer;
viewer.setUpViewInWindow(200, 200, 800, 600);
osg::ref_ptr<KeyboardHandler> keyboardEvent(new KeyboardHandler());
viewer.addEventHandler(keyboardEvent.get());
osg::ref_ptr<osg::Camera> camera = viewer.getCamera();
#endif*/
// parse robot message
robogenMessage::Robot robotMessage = robotRep.serialize();
// parse robot
boost::shared_ptr<Robot> robot(new Robot);
if (!robot->init(odeWorld, odeSpace, robotMessage)) {
std::cout << "Problem when initializing robot in body verifier!"
<< std::endl;
success = false;
}
if (success) {
std::vector<boost::shared_ptr<Model> > bodyParts = robot->getBodyParts();
/* no need for this
#ifdef VISUAL_DEBUG
// body part rendering
osg::ref_ptr<osg::Group> root = osg::ref_ptr<osg::Group>(new osg::Group);
std::vector<boost::shared_ptr<RenderModel> > renderModels;
for (unsigned int i = 0; i < bodyParts.size(); ++i) {
boost::shared_ptr<RenderModel> renderModel =
RobogenUtils::createRenderModel(bodyParts[i]);
if (renderModel == NULL) {
std::cout << "Cannot create a render model for model " << i
<< std::endl;
}
if (!renderModel->initRenderModel()) {
std::cout
<< "Cannot initialize a render model for one of the components. "
<< std::endl
<< "Please check that the models/ folder exists in the parent folder of this executable."
<< std::endl;
}
renderModels.push_back(renderModel);
root->addChild(renderModels[i]->getRootNode());
}
// viewer setup
viewer.setSceneData(root.get());
viewer.realize();
if (!viewer.getCameraManipulator()
&& viewer.getCamera()->getAllowEventFocus()) {
viewer.setCameraManipulator(new osgGA::TrackballManipulator());
}
viewer.setReleaseContextAtEndOfFrameHint(false);
#endif*/
// build map from ODE bodies to body part ID's: needed to identify
// offending body parts
std::map<dBodyID, std::string> dBodyToPartID;
for (unsigned int i = 0; i < bodyParts.size(); ++i) {
std::vector<dBodyID> dBodies = bodyParts[i]->getBodies();
for (unsigned int j = 0; j < dBodies.size(); ++j) {
dBodyToPartID[dBodies[j]] = bodyParts[i]->getId();
}
}
dSpaceCollide(odeSpace, (void *) collisionData, collisionCallback);
if (collisionData->offendingBodies.size()) {
success = false;
errorCode = SELF_INTERSECTION;
} else if(collisionData->cylinders.size() > 0) {
// hack to make sure we have separation between wheels
for(unsigned int i=0; i<collisionData->cylinders.size(); ++i) {
dReal radius;
dReal length;
dGeomCylinderGetParams(collisionData->cylinders[i],
&radius, &length);
dGeomCylinderSetParams(collisionData->cylinders[i],
radius + WHEEL_SEPARATION, length);
}
dSpaceCollide(odeSpace, (void *) collisionData, collisionCallback);
if ( collisionData->offendingBodies.size() ) {
success = false;
errorCode = SELF_INTERSECTION;
}
collisionData->cylinders.clear();
}
for (unsigned int i = 0; i < collisionData->offendingBodies.size(); i++) {
// TODO unlikely event that body not found in map?
affectedBodyParts.push_back(
std::pair<std::string, std::string>(
dBodyToPartID[collisionData->offendingBodies[i].first],
dBodyToPartID[collisionData->offendingBodies[i].second
]));
}
/*
#ifdef VISUAL_DEBUG
// show robot in viewer
while (!keyboardEvent->isQuit() && !viewer.done()) {
viewer.frame();
};
#endif certainly don't need */
}
// destruction
collisionData->offendingBodies.clear();
delete collisionData;
robot.reset();
dSpaceDestroy(odeSpace);
dWorldDestroy(odeWorld);
dCloseODE();
return success;
}
int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (Cylinder->type == dCylinderClass);
dIASSERT (Sphere->type == dSphereClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
//unsigned char* pContactData = (unsigned char*)contact;
int GeomCount = 0; // count of used contacts
#ifdef dSINGLE
const dReal toleranz = REAL(0.0001);
#endif
#ifdef dDOUBLE
const dReal toleranz = REAL(0.0000001);
#endif
// get the data from the geoms
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
//const dReal* pfRot1 = dGeomGetRotation(Cylinder);
dReal radius2;
radius2 = dGeomSphereGetRadius(Sphere);
const dReal* SpherePos = dGeomGetPosition(Sphere);
// G1Pos1 is the middle of the first disc
// G1Pos2 is the middle of the second disc
// vDir1 is the unit direction of the cylinderaxis
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = length * REAL(0.5); // just a precomputed factor
G1Pos2[0] = vDir1[0] * s + cylpos[0];
G1Pos2[1] = vDir1[1] * s + cylpos[1];
G1Pos2[2] = vDir1[2] * s + cylpos[2];
G1Pos1[0] = vDir1[0] * -s + cylpos[0];
G1Pos1[1] = vDir1[1] * -s + cylpos[1];
G1Pos1[2] = vDir1[2] * -s + cylpos[2];
dVector3 C;
dReal t;
// Step 1: compute the two distances 's' and 't'
// 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
if(s < (-radius2) || s > (length + radius2) )
{
// Sphere is too far away from the discs
// no collision
return 0;
}
// C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
// t is the distance from the Sphere-middle to the cylinder-axis!
t = dVector3Length(C);
if(t > (radius + radius2) )
{
// Sphere is too far away from the cylinder axis!
// no collision
return 0;
}
// decide which kind of collision we have:
if(t > radius && (s < 0 || s > length) )
{
// 3. collision
if(s <= 0)
{
contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
else
{
// now s is bigger than length here!
contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
else if( (radius - t) <= s && (radius - t) <= (length - s) )
{
// 1. collsision
if(t > (radius2 + toleranz))
{
// cylinder-axis is outside the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
C[0] /= t;
C[1] /= t;
C[2] /= t;
contact->pos[0] = C[0] * radius2 + SpherePos[0];
contact->pos[1] = C[1] * radius2 + SpherePos[1];
contact->pos[2] = C[2] * radius2 + SpherePos[2];
contact->normal[0] = C[0];
contact->normal[1] = C[1];
contact->normal[2] = C[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
else
{
// cylinder-axis is outside of the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
contact->pos[0] = C[0] + SpherePos[0];
contact->pos[1] = C[1] + SpherePos[1];
contact->pos[2] = C[2] + SpherePos[2];
contact->normal[0] = C[0] / t;
contact->normal[1] = C[1] / t;
contact->normal[2] = C[2] / t;
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
}
else
{
// 2. collision
if(s <= (length * REAL(0.5)) )
{
// collsision with the first disc
contact->depth = s + radius2;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
contact->normal[0] = vDir1[0];
contact->normal[1] = vDir1[1];
contact->normal[2] = vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
else
{
// collsision with the second disc
contact->depth = (radius2 + length - s);
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
contact->normal[0] = -vDir1[0];
contact->normal[1] = -vDir1[1];
contact->normal[2] = -vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
return GeomCount;
}
int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
int /*flags*/, dContactGeom *contact, int /*skip*/)
{
int GeomCount = 0; // count of used contacts
const dReal toleranz = REAL(0.0001f);
// get the data from the geoms
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
const dReal* pfRot1 = dGeomGetRotation(Cylinder);
dReal radius2;
radius2 = dGeomSphereGetRadius(Sphere);
const dReal* SpherePos = dGeomGetPosition(Sphere);
// G1Pos1 is the middle of the first disc
// G1Pos2 is the middle of the second disc
// vDir1 is the unit direction of the cylinderaxis
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = dMUL(length,REAL(0.5)); // just a precomputed factor
G1Pos2[0] = dMUL(vDir1[0],s) + cylpos[0];
G1Pos2[1] = dMUL(vDir1[1],s) + cylpos[1];
G1Pos2[2] = dMUL(vDir1[2],s) + cylpos[2];
G1Pos1[0] = dMUL(vDir1[0],-s) + cylpos[0];
G1Pos1[1] = dMUL(vDir1[1],-s) + cylpos[1];
G1Pos1[2] = dMUL(vDir1[2],-s) + cylpos[2];
dVector3 C;
dReal t;
// Step 1: compute the two distances 's' and 't'
// 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
s = dMUL((SpherePos[0] - G1Pos1[0]),vDir1[0]) - dMUL((G1Pos1[1] - SpherePos[1]),vDir1[1]) - dMUL((G1Pos1[2] - SpherePos[2]),vDir1[2]);
if(s < (-radius2) || s > (length + radius2) )
{
// Sphere is too far away from the discs
// no collision
return 0;
}
// C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
C[0] = dMUL(s,vDir1[0]) + G1Pos1[0] - SpherePos[0];
C[1] = dMUL(s,vDir1[1]) + G1Pos1[1] - SpherePos[1];
C[2] = dMUL(s,vDir1[2]) + G1Pos1[2] - SpherePos[2];
// t is the distance from the Sphere-middle to the cylinder-axis!
t = dSqrt(dMUL(C[0],C[0]) + dMUL(C[1],C[1]) + dMUL(C[2],C[2]) );
if(t > (radius + radius2) )
{
// Sphere is too far away from the cylinder axis!
// no collision
return 0;
}
// decide which kind of collision we have:
if(t > radius && (s < 0 || s > length) )
{
// 3. collision
if(s <= 0)
{
contact->depth = radius2 - dSqrt( dMUL(s,s) + dMUL((t - radius),(t - radius)) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = dMUL(dDIV(C[0],t),-radius) + G1Pos1[0];
contact->pos[1] = dMUL(dDIV(C[1],t),-radius) + G1Pos1[1];
contact->pos[2] = dMUL(dDIV(C[2],t),-radius) + G1Pos1[2];
contact->normal[0] = dDIV((contact->pos[0] - SpherePos[0]),(radius2 - contact->depth));
contact->normal[1] = dDIV((contact->pos[1] - SpherePos[1]),(radius2 - contact->depth));
contact->normal[2] = dDIV((contact->pos[2] - SpherePos[2]),(radius2 - contact->depth));
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
else
{
// now s is bigger than length here!
contact->depth = radius2 - dSqrt( dMUL((s - length),(s - length)) + dMUL((t - radius),(t - radius)) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = dMUL(dDIV(C[0],t),-radius) + G1Pos2[0];
contact->pos[1] = dMUL(dDIV(C[1],t),-radius) + G1Pos2[1];
contact->pos[2] = dMUL(dDIV(C[2],t),-radius) + G1Pos2[2];
contact->normal[0] = dDIV((contact->pos[0] - SpherePos[0]),(radius2 - contact->depth));
contact->normal[1] = dDIV((contact->pos[1] - SpherePos[1]),(radius2 - contact->depth));
contact->normal[2] = dDIV((contact->pos[2] - SpherePos[2]),(radius2 - contact->depth));
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
else if( (radius - t) <= s && (radius - t) <= (length - s) )
{
// 1. collsision
if(t > (radius2 + toleranz))
{
// cylinder-axis is outside the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
C[0] = dDIV(C[0],t);
C[1] = dDIV(C[1],t);
C[2] = dDIV(C[2],t);
contact->pos[0] = dMUL(C[0],radius2) + SpherePos[0];
contact->pos[1] = dMUL(C[1],radius2) + SpherePos[1];
contact->pos[2] = dMUL(C[2],radius2) + SpherePos[2];
contact->normal[0] = C[0];
contact->normal[1] = C[1];
contact->normal[2] = C[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
else
{
// cylinder-axis is outside of the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
contact->pos[0] = C[0] + SpherePos[0];
contact->pos[1] = C[1] + SpherePos[1];
contact->pos[2] = C[2] + SpherePos[2];
contact->normal[0] = dDIV(C[0],t);
contact->normal[1] = dDIV(C[1],t);
contact->normal[2] = dDIV(C[2],t);
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
}
else
{
// 2. collision
if(s <= dMUL(length,REAL(0.5)) )
{
// collsision with the first disc
contact->depth = s + radius2;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = dMUL(radius2,vDir1[0]) + SpherePos[0];
contact->pos[1] = dMUL(radius2,vDir1[1]) + SpherePos[1];
contact->pos[2] = dMUL(radius2,vDir1[2]) + SpherePos[2];
contact->normal[0] = vDir1[0];
contact->normal[1] = vDir1[1];
contact->normal[2] = vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
else
{
// collsision with the second disc
contact->depth = (radius2 + length - s);
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = dMUL(radius2,-vDir1[0]) + SpherePos[0];
contact->pos[1] = dMUL(radius2,-vDir1[1]) + SpherePos[1];
contact->pos[2] = dMUL(radius2,-vDir1[2]) + SpherePos[2];
contact->normal[0] = -vDir1[0];
contact->normal[1] = -vDir1[1];
contact->normal[2] = -vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
}
// initialize collision data
void _cldInitCylinderBox(sCylinderBoxData& cData)
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(cData.gBox);
dMatrix3Copy(pRotBox,cData.mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
dVector3Copy(*pPosBox,cData.vBoxPos);
dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
cData.vBoxHalfSize[0] *= REAL(0.5);
cData.vBoxHalfSize[1] *= REAL(0.5);
cData.vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
// vertex 1
cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
// vertex 2
cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
// vertex 3
cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
// vertex 4
cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
// vertex 5
cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
// vertex 6
cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
// vertex 7
cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
}
// find relative position
dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
cData.fBestDepth = MAX_FLOAT;
cData.vNormal[0] = REAL(0.0);
cData.vNormal[1] = REAL(0.0);
cData.vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = M_PI/nCYLINDER_SEGMENT;
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
cData.fBestrb = 0;
cData.fBestrc = 0;
cData.iBestAxis = 0;
cData.nContacts = 0;
}
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (Cylinder->type == dCylinderClass);
dIASSERT (Plane->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
int GeomCount = 0; // count of used contactgeoms
#ifdef dSINGLE
const dReal toleranz = REAL(0.0001);
#endif
#ifdef dDOUBLE
const dReal toleranz = REAL(0.0000001);
#endif
// Get the properties of the cylinder (length+radius)
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
// and the plane
dVector4 planevec;
dGeomPlaneGetParams(Plane, planevec);
dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = length * REAL(0.5);
G1Pos2[0] = vDir1[0] * s + cylpos[0];
G1Pos2[1] = vDir1[1] * s + cylpos[1];
G1Pos2[2] = vDir1[2] * s + cylpos[2];
G1Pos1[0] = vDir1[0] * -s + cylpos[0];
G1Pos1[1] = vDir1[1] * -s + cylpos[1];
G1Pos1[2] = vDir1[2] * -s + cylpos[2];
dVector3 C;
// parallel-check
s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
if(s < 0)
s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
else
s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
if(s < toleranz && s > (-toleranz))
{
// discs are parallel to the plane
// 1.compute if, and where contacts are
dVector3 P;
s = planevec[3] - dVector3Dot(planevec, G1Pos1);
dReal t;
t = planevec[3] - dVector3Dot(planevec, G1Pos2);
if(s >= t) // s == t does never happen,
{
if(s >= 0)
{
// 1. Disc
dVector3Copy(G1Pos1, P);
}
else
return GeomCount; // no contacts
}
else
{
if(t >= 0)
{
// 2. Disc
dVector3Copy(G1Pos2, P);
}
else
return GeomCount; // no contacts
}
// 2. generate a coordinate-system on the disc
dVector3 V1, V2;
if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
{
// not x-axis
V1[0] = vDir1[0] + REAL(1.0); // random value
V1[1] = vDir1[1];
V1[2] = vDir1[2];
}
else
{
// maybe x-axis
V1[0] = vDir1[0];
V1[1] = vDir1[1] + REAL(1.0); // random value
V1[2] = vDir1[2];
}
// V1 is now another direction than vDir1
// Cross-product
dVector3Cross(V1, vDir1, V2);
// make unit V2
t = dVector3Length(V2);
t = radius / t;
dVector3Scale(V2, t);
// cross again
dVector3Cross(V2, vDir1, V1);
// |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
// V1 = first axis
// V2 = second axis
// 3. generate contactpoints
// Potential contact 1
dVector3Add(P, V1, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// Potential contact 2
dVector3Subtract(P, V1, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// Potential contact 3
dVector3Add(P, V2, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// Potential contact 4
dVector3Subtract(P, V2, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
}
else
{
dReal t = dVector3Dot(PlaneNormal, vDir1);
C[0] = vDir1[0] * t - PlaneNormal[0];
C[1] = vDir1[1] * t - PlaneNormal[1];
C[2] = vDir1[2] * t - PlaneNormal[2];
s = dVector3Length(C);
// move C onto the circle
s = radius / s;
dVector3Scale(C, s);
// deepest point of disc 1
dVector3Add(C, G1Pos1, contact->pos);
// depth of the deepest point
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth >= 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// C is still computed
// deepest point of disc 2
dVector3Add(C, G1Pos2, contact->pos);
// depth of the deepest point
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth >= 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
}
return GeomCount;
}
void PhyCylinder::GetCylinderParam(float & radius, float & length)
{
dGeomCylinderGetParams(mOdeGeom, &radius, &length);
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
//<---- Convex Object
else if (type == dConvexClass)
{
#if 0
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
#else
dsDrawConvex(pos,R,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_body) {
dBodyID body = dGeomGetBody(g);
if (body) {
const dReal *bodypos = dBodyGetPosition (body);
const dReal *bodyr = dBodyGetRotation (body);
dReal bodySides[3] = { 0.1, 0.1, 0.1 };
dsSetColorAlpha(0,1,0,1);
dsDrawBox(bodypos,bodyr,bodySides);
}
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void Simulator::DrawGeometry(dGeomID geom)
{
if(dGeomGetData(geom) == ((void *) &TYPE_OBSTACLE))
glColor3d(0.8, 0.2, 0.2);
else if(dGeomGetData(geom) == ((void *) &TYPE_TERRAIN))
glColor3d(0.4, 0.5, 0.7);
else //type robot
glColor3d(0.0, 0, 1.0);
const int type = dGeomGetClass(geom);
const dReal *pos = dGeomGetPosition(geom);
const dReal *rot = dGeomGetRotation(geom);
double m[16];
glPushMatrix();
//transform position and orientation into an OpenGL matrix
m[0] = rot[0];
m[1] = rot[4];
m[2] = rot[8];
m[4] = rot[1];
m[5] = rot[5];
m[6] = rot[9];
m[8] = rot[2];
m[9] = rot[6];
m[10] = rot[10];
m[3] = m[7] = m[11] = 0;
m[15] = 1;
m[12] = pos[0];
m[13] = pos[1];
m[14] = pos[2];
glMultMatrixd(m);
if(type == dBoxClass)
{
dVector3 lengths;
dGeomBoxGetLengths(geom, lengths);
glPushMatrix();
glScaled(lengths[0], lengths[1], lengths[2]);
glutSolidCube(1.0);
glPopMatrix();
}
else if(type == dSphereClass)
glutSolidSphere(dGeomSphereGetRadius(geom), 20, 20);
else if(type == dCylinderClass)
{
dReal r, length;
dGeomCylinderGetParams(geom, &r, &length);
glTranslated(0, 0, -0.5 * length);
static GLUquadric *gluQuadric = NULL;
if(gluQuadric == NULL)
gluQuadric = gluNewQuadric();
gluCylinder(gluQuadric, r, r, length, 20, 20);
}
glPopMatrix();
}
//! setting of scale
void SSimEntity::setScale(Vector3d scale)
{
m_scale.set(scale.x(), scale.y(), scale.z());
// if a part is already added
int geomNum = getGeomNum();
if (geomNum != 0) {
dBodyID body = m_parts.body;
// loop for all of the parts
for (int i = 0; i < geomNum; i++) {
// refer the geometry
dGeomID geom = m_parts.geoms[i];
// refer the position (gap from CoG)
const dReal *pos = dGeomGetPosition(geom);
// Reflection of scale
dGeomSetPosition(geom, pos[0]*scale.x(), pos[1]*scale.y(), pos[2]*scale.z());
// Refer the type of the geometory
int type = dGeomGetClass(geom);
// setting of mass
// sphere
if (type == 0) {
// average of scale
double mean = (scale.x() + scale.y() + scale.z())/ 3;
// refer the radius
dReal radius = dGeomSphereGetRadius(geom);
// reflection of scale
dGeomSphereSetRadius(geom, radius*mean);
}
// box
else if (type == 1) {
// refer the size
dVector3 size;
dGeomBoxGetLengths(geom, size);
// reflection of scale
dGeomBoxSetLengths(geom, size[0]*scale.x(), size[1]*scale.y(), size[2]*scale.z());
}
// cylinder
else if (type == 3) {
dReal radius, length;
dGeomCylinderGetParams(geom, &radius, &length);
// average of scale in horizontal plane
double mean = (scale.x() + scale.z()) / 2;
// TODO: confirm: is 2 suitable for long axis?
dGeomCylinderSetParams(geom, radius*mean, length*scale.y());
}
} // for (int i = 0; i < partsNum; i++) {
}
}
//! add ODE parts with fixed joint
void SSimEntity::setMass(double mass)
{
m_parts.mass = mass;
int geomNum = getGeomNum();
if (geomNum != 0) {
// if a part is already added
// mass per part
double ms = mass / geomNum;
// refer geometry and body
dBodyID body = m_parts.body;
dMass m;
dMass m2;
dMassSetZero(&m); dMassSetZero(&m2);
for (int i = 0; i < geomNum; i++) {
// refer the type of geometory
dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]);
int type = dGeomGetClass(geom);
// setting of mass
// sphere
if (type == 0) {
dReal radius = dGeomSphereGetRadius(geom);
dMassSetSphereTotal(&m2, ms, radius);
}
// box
else if (type == 1) {
dVector3 size;
dGeomBoxGetLengths(geom, size);
dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]);
}
// cylinder
else if (type == 3) {
dReal radius = 0.0;
dReal length = 0.0;
dGeomCylinderGetParams(geom, &radius, &length);
// TODO: confirm: Is 2 suitable for long axis?
dMassSetCylinderTotal(&m2, ms, 2, radius, length);
}
const dReal *pos = dGeomGetPosition(geom);
dMassTranslate(&m2, pos[0], pos[1], pos[2]);
dMassAdd(&m, &m2);
}
/*
for (int i = 0; i < geomNum; i++) {
dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]);
const dReal *pos = dGeomGetPosition(geom);
// Change to a relative position from CoG
dGeomSetPosition(geom, pos[0] - m.c[0], pos[1] - m.c[1], pos[2] - m.c[2]);
}
// keep the CoG position
//m_parts.pos.set(m.c[0], m.c[1], m.c[2]);
*/
// Adjustment of the gap from CoG
//const dReal *p = dBodyGetPosition(body);
//dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]);
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
// Setting of mass
dBodySetMass(body, &m);
dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test
} // if (partsNum != 0) {
}
