C++ Transform3d::transform_point方法代码示例

/// Transforms the given triangle using the specified pose 
Triangle Triangle::transform(const Triangle& t, const Transform3d& m)
{
  Point3d a = m.transform_point(t.a);
  Point3d b = m.transform_point(t.b);
  Point3d c = m.transform_point(t.c);
  
  return Triangle(a, b, c);
}

// setup simulator callback
void post_step_callback(Simulator* sim)
{
  const unsigned X = 0, Y = 1, Z = 2;

  // setup the box height 
  const double H = 1.0;

  // get the bottoms of the box 
  Transform3d wTs = Pose3d::calc_relative_pose(box->get_pose(), GLOBAL);
  Vector3d p1 = wTs.transform_point(Vector3d(-.5, -.5, -.5, box->get_pose()));
  Vector3d p2 = wTs.transform_point(Vector3d(-.5, -.5, .5, box->get_pose()));
  Vector3d p3 = wTs.transform_point(Vector3d(.5, -.5, -.5, box->get_pose()));
  Vector3d p4 = wTs.transform_point(Vector3d(.5, -.5, .5, box->get_pose()));

  // get the bottoms of the box in the global frame
  shared_ptr<Pose3d> P1(new Pose3d), P2(new Pose3d), P3(new Pose3d), P4(new Pose3d);  
  P1->x = Origin3d(p1);
  P2->x = Origin3d(p2);
  P3->x = Origin3d(p3);
  P4->x = Origin3d(p4);

  // get the velocity of the box at the contact points
  SVelocityd v = box->get_velocity();
  Vector3d xd1 = Pose3d::calc_relative_pose(v.pose, P1).transform(v).get_linear();
  Vector3d xd2 = Pose3d::calc_relative_pose(v.pose, P2).transform(v).get_linear();
  Vector3d xd3 = Pose3d::calc_relative_pose(v.pose, P3).transform(v).get_linear();
  Vector3d xd4 = Pose3d::calc_relative_pose(v.pose, P4).transform(v).get_linear();

/*
  SVelocityd v = box->get_velocity();
  Origin3d xd(v.get_linear());
  Origin3d omega(v.get_angular());
  Origin3d s(1.0, 0.0, 0.0);
  Origin3d t(0.0, 1.0, 0.0);
  Origin3d crosss = Origin3d::cross(-wTs.x, s);
  Origin3d crosst = Origin3d::cross(-wTs.x, t);
*/

  // output the sliding velocity at the contact 
  std::ofstream out("contactv.dat", std::ostream::app);
  out << sim->current_time << " " << xd1[X] << " " << xd1[Y] << " " << xd2[X] << " " << xd2[Y] << " " << xd3[X] << " " << xd3[Y] << " " << xd4[X] << " " << xd4[Y] << std::endl;
//  out << sim->current_time << " " << (s.dot(xd) + crosss.dot(omega)) << " " << (t.dot(xd) + crosst.dot(omega)) << std::endl; 
//  out << sim->current_time << " " << v[3] << " " << v[4] << " " << v[5] << " " << v[0] << " " << v[1] << " " << v[2] << std::endl;
  out.close();

  out.open("ke.dat", std::ostream::app);
  out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl;
  out.close();
}

/// Tests intersection between a SSR and a SSL
bool BV::intersects(const SSR* A, const SSL* B, const Transform3d& aTb)
{
  Point3d Bp1 = aTb.transform_point(B->p1);
  Point3d Bp2 = aTb.transform_point(B->p2);
  double dist = SSR::calc_dist(*A, LineSeg3(Bp1, Bp2));
  return dist <= B->radius;
}

/// Computes the distance from another sphere primitive
double SpherePrimitive::calc_signed_dist(shared_ptr<const SpherePrimitive> s, Point3d& pthis, Point3d& ps) const
{
  // get the transform from s to this
  Transform3d T = Pose3d::calc_relative_pose(ps.pose, pthis.pose);

  // compute the distance
  double d = T.x.norm() - _radius - s->_radius;

  // setup sphere centers in alternate frames
  Point3d ps_c(0.0, 0.0, 0.0, ps.pose);
  Point3d pthis_c(0.0, 0.0, 0.0, pthis.pose);

  // setup closest points
  pthis = T.transform_point(ps_c);
  pthis.normalize();
  ps = T.inverse_transform_point(pthis_c);
  ps.normalize();

  // scale closest points appropriately
  if (d > 0.0)
  {
    pthis *= _radius;
    ps *= s->_radius;
  }
  else
  {
    pthis *= _radius+d;
    ps *= s->_radius+d;
  }

  return d;
}

/// Gets the distance from a sphere primitive
double PlanePrimitive::calc_signed_dist(shared_ptr<const SpherePrimitive> s, Point3d& pthis, Point3d& ps) const
{
  const unsigned Y = 1;

  assert(_poses.find(const_pointer_cast<Pose3d>(pthis.pose)) != _poses.end());

  // compute the transform from the sphere to the plane
  Transform3d T = Pose3d::calc_relative_pose(ps.pose, pthis.pose);

  // transform the sphere center to the plane space
  Point3d ps_c(0.0, 0.0, 0.0, ps.pose);
  Point3d ps_c_this = T.transform_point(ps_c);

  // get the lowest point on the sphere (toward the heightmap)
  Vector3d vdir(0.0, -1.0*s->get_radius(), 0.0, pthis.pose);

  // get the lowest point on the sphere
  Point3d sphere_lowest = ps_c_this + vdir;

  // setup the point on the plane
  pthis = ps_c_this;
  pthis[Y] = 0.0;

  // get the height of the sphere center
  ps = T.inverse_transform_point(sphere_lowest);
  return sphere_lowest[Y];
}

/// Transforms the BoundingSphere using the given transform
void BoundingSphere::transform(const Transform3d& T, BV* result) const
{
  // get the BoundingSphere 
  BoundingSphere& s = *((BoundingSphere*) result);

  // copy this
  s = *this;

  // transform the center
  s.center = T.transform_point(center);
}