Exemple #1
0
    /// Finds contacts between the ball and a plane
    virtual void find_contacts_ball_parabloid(CollisionGeometryPtr ball_cg, CollisionGeometryPtr parabloid_cg, std::vector<UnilateralConstraint>& contacts)
    {
      const unsigned X = 0, Y = 1, Z = 2;

      // get the pose for the parabloid 
      shared_ptr<const Pose3d> Pparabloid = parabloid_cg->get_pose(); 

      // get the pose for the ball 
      shared_ptr<const Pose3d> Pball = ball_cg->get_pose();

      // get the pose of the center-of-mass of the ball
      Transform3d pTb = Pose3d::calc_relative_pose(Pball, Pparabloid);

      // compute the height of the ball above the parabloid; note: this assumes
      // that the parabloid is relatively flat
      double dist = -sqr(pTb.x[X])/(A*A) + sqr(pTb.x[Y])/(B*B) + (pTb.x[Z] - R); 

      // compute the closest point - we'll go half way between the closest
      // point on the ball and the closest point on the surface 
      double z = sqr(pTb.x[X])/sqr(A) + sqr(pTb.x[Y])/sqr(B);
      Point3d closest_parabloid(pTb.x[X], pTb.x[Y], z, Pparabloid);
      Point3d closest_ball(0.0, 0.0, -R, Pball);
      Point3d point = (Pose3d::transform_point(GLOBAL, closest_ball) +
                       Pose3d::transform_point(GLOBAL, closest_parabloid))*0.5; 

      // compute the normal to the surface
      Vector3d normal(2.0*-pTb.x[X]/(A*A), 2.0*-pTb.x[Y]/(B*B), 1.0, Pparabloid); 
      normal.normalize();
      Vector3d normal_global = Pose3d::transform_vector(GLOBAL, normal);

      contacts.clear();
      contacts.push_back(
        CollisionDetection::create_contact(ball_cg,parabloid_cg,point, normal_global,dist)
              );
    }
Exemple #2
0
    /// Calculates signed distance between a ball and a plane
    double calc_signed_dist_ball_parabloid(CollisionGeometryPtr ball_cg, CollisionGeometryPtr parabloid_cg,Vector3d& pball,Vector3d& pparabloid)
    {
      const unsigned X = 0, Y = 1, Z = 2;

      // setup the minimum dist
      double min_dist = std::numeric_limits<double>::max();

      // get the pose for the parabloid 
      shared_ptr<const Pose3d> Pparabloid = parabloid_cg->get_pose(); 

      // get the pose for the ball 
      shared_ptr<const Pose3d> Pball = ball_cg->get_pose();

      // get the pose of the center-of-mass of the ball
      Transform3d pTb = Pose3d::calc_relative_pose(Pball, Pparabloid);

      // compute the height of the ball above the parabloid; note: this assumes
      // that the parabloid is relatively flat
      double dist = -sqr(pTb.x[X])/(A*A) + sqr(pTb.x[Y])/(B*B) + (pTb.x[Z] - R); 

      // compute the closest point - we'll go half way between the closest
      // point on the ball and the closest point on the surface 
      double z = sqr(pTb.x[X])/sqr(A) + sqr(pTb.x[Y])/sqr(B);
      pparabloid = Point3d(pTb.x[X], pTb.x[Y], z, Pparabloid);
      pball = Point3d(0.0, 0.0, -R, Pball);

      return dist;
    }