C++ (Cpp) ccdQuatSetの例

コード例 #1

ファイル: gjk_libccd.cpp プロジェクト: ktossell/fcl

/** 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);
}

コード例 #2

ファイル: collision_libccd.cpp プロジェクト: JdeRobot/ThirdParty

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);
}

コード例 #3

ファイル: gjk_libccd.cpp プロジェクト: ktossell/fcl

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;
}

コード例 #4

ファイル: gjk_libccd.cpp プロジェクト: ktossell/fcl

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;
}