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