C++ (Cpp) Cone::getLocalTranslationの例

コード例 #1

ファイル: geometric_shapes_utility.cpp プロジェクト: Gitu1997/Gazebo

void computeBV<OBB>(const Cone& s, OBB& bv)
{
  Vec3f R[3];
  matMulMat(s.getRotation(), s.getLocalRotation(), R);
  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();

  bv.To = T;
  bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
  bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
  bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
  bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
}

コード例 #2

ファイル: geometric_shapes_utility.cpp プロジェクト: Gitu1997/Gazebo

void computeBV<AABB>(const Cone& s, AABB& bv)
{
  Vec3f R[3];
  matMulMat(s.getRotation(), s.getLocalRotation(), R);
  Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();

  BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
  BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
  BVH_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);

  bv.max_ = T + Vec3f(x_range, y_range, z_range);
  bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
}