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