/// 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;
}
/// Determines whether a pair of geometries is checked for collision detection
bool CollisionDetection::is_checked(CollisionGeometryPtr cg1, CollisionGeometryPtr cg2) const
{
// if both geometries belong to one rigid body, don't check
RigidBodyPtr rb1 = dynamic_pointer_cast<RigidBody>(cg1->get_single_body());
RigidBodyPtr rb2 = dynamic_pointer_cast<RigidBody>(cg2->get_single_body());
if (rb1 && rb2 && rb1 == rb2)
return false;
// check the geometries
return (is_enabled(cg1) && is_enabled(cg2) && is_enabled(cg1, cg2));
}
/// Calculates the signed distances between two geometries and returns closest points if geometries are not interpenetrating
double CollisionGeometry::calc_signed_dist(CollisionGeometryPtr gA, CollisionGeometryPtr gB, Point3d& pA, Point3d& pB)
{
// get the two primitives
PrimitivePtr primA = gA->get_geometry();
PrimitivePtr primB = gB->get_geometry();
// setup poses for the points
pA.pose = primA->get_pose(gA);
pB.pose = primB->get_pose(gB);
FILE_LOG(LOG_COLDET) << "CollisionGeometry::calc_signed_dist() - computing signed distance between " << gA->get_single_body()->id << " and " << gB->get_single_body()->id << std::endl;
// now compute the signed distance
return primA->calc_signed_dist(primB, pA, pB);
}
/// Calculates the velocity expanded bounding volume for the bounding sphere (calculates an OBB)
BVPtr BoundingSphere::calc_swept_BV(CollisionGeometryPtr g, const SVelocityd& v) const
{
// get the corresponding body
SingleBodyPtr b = g->get_single_body();
// if the body does not move, just return the bounding sphere
if (!b->is_enabled())
{
FILE_LOG(LOG_BV) << "BoundingSphere::calc_swept_BV() entered" << endl;
FILE_LOG(LOG_BV) << " -- using original bounding sphere" << endl;
FILE_LOG(LOG_BV) << "BoundingSphere::calc_swept_BV() exited" << endl;
return const_pointer_cast<BoundingSphere>(get_this());
}
// get the velocity in the proper frame
SVelocityd vx = Pose3d::transform(get_relative_pose(), v);
// otherwise, create a SSL
shared_ptr<SSL> ssl(new SSL);
ssl->p1 = this->center;
ssl->p2 = this->center + vx.get_linear();
ssl->radius = this->radius;
return ssl;
}
void ColdetPlugin::add_collision_geometry(CollisionGeometryPtr cg)
{
RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(cg->get_single_body());
if (rb->id.find("box") != std::string::npos)
{
// add the box
_boxes.push_back(cg);
// sort all collision geometries
std::map<std::string, CollisionGeometryPtr> geoms;
for (unsigned i=0; i< _boxes.size(); i++)
geoms[_boxes[i]->get_single_body()->id] = _boxes[i];
// clear the boxes
_boxes.clear();
for (std::map<std::string, CollisionGeometryPtr>::const_iterator i = geoms.begin(); i != geoms.end(); i++)
_boxes.push_back(i->second);
}
else
{
assert(rb->id == "ground");
_ground = cg;
}
}
/**
* This method looks for contact data not only between the pair of geometries, but also
* the rigid bodies that the geometries belong to, and any articulated bodies as well.
* The search proceeds in the following manner: <br />
* <ol>
* <li>two collision geometries</li>
* <li>one collision geometry, one rigid body</li>
* <li>two rigid bodies</li>
* <li>one collision geometry, one articulated body</li>
* <li>one rigid body, one articulated body</li>
* <li>two articulated bodies</li>
* </ol>
* The search order allows for multiple granularities; for example, a collision can easily
* be specified between two geometries of two of a robot's links (i.e., representing different
* surfaces on the links), between two links, or between two robots.
* \param g1 the first collision geometry
* \param g2 the second collision geometry
* \return a pointer to the contact data, if any, found
*/
shared_ptr<ContactParameters> EventDrivenSimulator::get_contact_parameters(CollisionGeometryPtr geom1, CollisionGeometryPtr geom2) const
{
map<sorted_pair<BasePtr>, shared_ptr<ContactParameters> >::const_iterator iter;
// search for the two contact geometries first
if ((iter = contact_params.find(make_sorted_pair(geom1, geom2))) != contact_params.end())
return iter->second;
// get the geometries as base pointers
BasePtr g1(geom1);
BasePtr g2(geom2);
// get the two single bodies
assert(geom1->get_single_body());
assert(geom2->get_single_body());
SingleBodyPtr singlebody1 = geom1->get_single_body();
SingleBodyPtr singlebody2 = geom2->get_single_body();
BasePtr sb1 = singlebody1;
BasePtr sb2 = singlebody2;
// search for contact geometry 1 and rigid body 2
if ((iter = contact_params.find(make_sorted_pair(g1, sb2))) != contact_params.end())
return iter->second;
// search for contact geometry 2 and rigid body 1
if ((iter = contact_params.find(make_sorted_pair(g2, sb1))) != contact_params.end())
return iter->second;
// search for both rigid bodies
if ((iter = contact_params.find(make_sorted_pair(sb1, sb2))) != contact_params.end())
return iter->second;
// get the articulated bodies, if any
RigidBodyPtr rb1 = dynamic_pointer_cast<RigidBody>(singlebody1);
RigidBodyPtr rb2 = dynamic_pointer_cast<RigidBody>(singlebody2);
BasePtr ab1, ab2;
if (rb1)
ab1 = rb1->get_articulated_body();
if (rb2)
ab2 = rb2->get_articulated_body();
// check collision geometry 2 and rigid body 2 against articulated body 1
if (ab1)
{
if ((iter = contact_params.find(make_sorted_pair(g2, ab1))) != contact_params.end())
return iter->second;
if ((iter = contact_params.find(make_sorted_pair(sb2, ab1))) != contact_params.end())
return iter->second;
}
// check collision geometry 1 and rigid body 1 against articulated body 2
if (ab2)
{
if ((iter = contact_params.find(make_sorted_pair(g1, ab2))) != contact_params.end())
return iter->second;
if ((iter = contact_params.find(make_sorted_pair(sb1, ab2))) != contact_params.end())
return iter->second;
}
// check the two articulated bodies against articulated body 2
if (ab1 && ab2)
if ((iter = contact_params.find(make_sorted_pair(ab1, ab2))) != contact_params.end())
return iter->second;
// still here? no contact data found
return shared_ptr<ContactParameters>();
}
