/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
{
const Quaternion3f& q = tf.getQuatRotation();
const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
ccdQuatInvert2(&o->rot_inv, &o->rot);
}
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);
}
void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3)
{
ccd_triangle_t* o = new ccd_triangle_t;
Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
ccdVec3Set(&o->c, center[0], center[1], center[2]);
ccdVec3Set(&o->pos, 0., 0., 0.);
ccdQuatSet(&o->rot, 0., 0., 0., 1.);
ccdQuatInvert2(&o->rot_inv, &o->rot);
return o;
}
void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf)
{
ccd_triangle_t* o = new ccd_triangle_t;
Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
ccdVec3Set(&o->c, center[0], center[1], center[2]);
const Quaternion3f& q = tf.getQuatRotation();
const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
ccdQuatInvert2(&o->rot_inv, &o->rot);
return o;
}