void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
{
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 == dCCylinderClass) {
dReal radius,length;
dGeomCCylinderGetParams (g,&radius,&length);
dsDrawCappedCylinder (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);
}
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!draw_geom){
return;
}
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 == dCCylinderClass) {
dReal radius,length;
dGeomCCylinderGetParams (g,&radius,&length);
dsDrawCappedCylinder (pos,R,length,radius);
}
/*
// cylinder option not yet implemented
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);
}
}
static void printCCylinder (PrintingContext &c, dxGeom *g)
{
dReal radius,length;
dGeomCCylinderGetParams (g,&radius,&length);
c.print ("type","capsule");
c.print ("radius",radius);
c.print ("length",length);
}
// capsule - trimesh By francisco leon
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dTriMeshClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)o1;
dxGeom* gCylinder = o2;
//Get capsule params
dMatrix3 mCapsuleRotation;
dVector3 vCapsulePosition;
dVector3 vCapsuleAxis;
dReal vCapsuleRadius;
dReal fCapsuleSize;
dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
memcpy(vCapsulePosition,pDst,sizeof(dVector3));
//Axis
vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
// Get size of CCylinder
dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
fCapsuleSize*=0.5f;
//Set Capsule params
GIM_CAPSULE_DATA capsule;
capsule.m_radius = vCapsuleRadius;
VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
//Collide trimeshe vs capsule
gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
if(trimeshcontacts.m_size == 0)
{
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return 0;
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
unsigned contactcount = trimeshcontacts.m_size;
unsigned contactmax = (unsigned)(flags & NUMC_MASK);
if (contactcount > contactmax)
{
contactcount = contactmax;
}
dContactGeom* pcontact;
unsigned i;
for (i=0;i<contactcount;i++)
{
pcontact = SAFECONTACT(flags, contact, i, skip);
pcontact->pos[0] = ptrimeshcontacts->m_point[0];
pcontact->pos[1] = ptrimeshcontacts->m_point[1];
pcontact->pos[2] = ptrimeshcontacts->m_point[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
pcontact->normal[3] = 0;
pcontact->depth = ptrimeshcontacts->m_depth;
pcontact->g1 = TriMesh;
pcontact->g2 = gCylinder;
pcontact->side1 = ptrimeshcontacts->m_feature1;
pcontact->side2 = -1;
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return (int)contactcount;
}
