/** Uses a raycast to see if anything has hit the rubber band.
* \param k Position of the kart = one end of the rubber band
* \param p Position of the plunger = other end of the rubber band.
*/
void RubberBand::checkForHit(const Vec3 &k, const Vec3 &p)
{
btCollisionWorld::ClosestRayResultCallback ray_callback(k, p);
// Disable raycast collision detection for this plunger and this kart!
short int old_plunger_group = m_plunger->getBody()->getBroadphaseHandle()->m_collisionFilterGroup;
short int old_kart_group=0;
// If the owner is being rescued, the broadphase handle does not exist!
if(m_owner->getBody()->getBroadphaseHandle())
old_kart_group = m_owner->getBody()->getBroadphaseHandle()->m_collisionFilterGroup;
m_plunger->getBody()->getBroadphaseHandle()->m_collisionFilterGroup = 0;
if(m_owner->getBody()->getBroadphaseHandle())
m_owner->getBody()->getBroadphaseHandle()->m_collisionFilterGroup = 0;
// Do the raycast
World::getWorld()->getPhysics()->getPhysicsWorld()->rayTest(k, p,
ray_callback);
// Reset collision groups
m_plunger->getBody()->getBroadphaseHandle()->m_collisionFilterGroup = old_plunger_group;
if(m_owner->getBody()->getBroadphaseHandle())
m_owner->getBody()->getBroadphaseHandle()->m_collisionFilterGroup = old_kart_group;
if(ray_callback.hasHit())
{
Vec3 pos(ray_callback.m_hitPointWorld);
UserPointer *up = (UserPointer*)ray_callback.m_collisionObject->getUserPointer();
if(up && up->is(UserPointer::UP_KART))
hit(up->getPointerKart(), &pos);
else
hit(NULL, &pos);
} // if raycast hast hit
} // checkForHit
bool BulletModel::collisionRaycast(const Matrix3Xd &origins, const Matrix3Xd &ray_endpoints, bool use_margins, VectorXd &distances)
{
distances.resize(origins.cols());
BulletCollisionWorldWrapper& bt_world = getBulletWorld(use_margins);
for (int i = 0; i < origins.cols(); i ++)
{
btVector3 ray_from_world(origins(0,i), origins(1,i), origins(2,i));
btVector3 ray_to_world(ray_endpoints(0,i), ray_endpoints(1,i), ray_endpoints(2,i));
btCollisionWorld::ClosestRayResultCallback ray_callback(ray_from_world, ray_to_world);
bt_world.bt_collision_world->rayTest(ray_from_world, ray_to_world, ray_callback);
if (ray_callback.hasHit()) {
// compute distance to hit
btVector3 end = ray_callback.m_hitPointWorld;
Vector3d end_eigen(end.getX(), end.getY(), end.getZ());
distances(i) = (end_eigen - origins.col(i)).norm();
} else {
distances(i) = -1;
}
}
return true;
}
/** Casts a ray from 'from' to 'to'. If a triangle of this mesh was hit,
* xyz and material will be set.
* \param from/to The from and to position for the raycast.
* \param xyz The position in world where the ray hit.
* \param material The material of the mesh that was hit.
* \param normal The intrapolated normal at that position.
* \param interpolate_normal If true, the returned normal is the interpolated
* based on the three normals of the triangle and the location of the
* hit point (which is more compute intensive, but results in much
* smoother results).
* \return True if a triangle was hit, false otherwise (and no output
* variable will be set.
*/
bool TriangleMesh::castRay(const btVector3 &from, const btVector3 &to,
btVector3 *xyz, const Material **material,
btVector3 *normal, bool interpolate_normal) const
{
if(!m_collision_shape)
{
*material=NULL;
return false;
}
btTransform trans_from;
trans_from.setIdentity();
trans_from.setOrigin(from);
btTransform trans_to;
trans_to.setIdentity();
trans_to.setOrigin(to);
btTransform world_trans;
// If there is a body, take the current transform from the body.
if(m_body)
world_trans = m_body->getWorldTransform();
else
world_trans.setIdentity();
btCollisionWorld::ClosestRayResultCallback result(from, to);
/** A special ray result class that stores the index of the triangle
* that was hit. */
class MaterialRayResult : public btCollisionWorld::ClosestRayResultCallback
{
public:
/** Stores the index of the triangle that was hit. */
int m_index;
// --------------------------------------------------------------------
MaterialRayResult(const btVector3 &p1, const btVector3 &p2,
const TriangleMesh *me)
: btCollisionWorld::ClosestRayResultCallback(p1,p2)
{
m_index = -1;;
} // MaterialRayResult
// --------------------------------------------------------------------
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,
bool normalInWorldSpace)
{
m_index = rayResult.m_localShapeInfo->m_triangleIndex;
return btCollisionWorld::ClosestRayResultCallback
::addSingleResult(rayResult, normalInWorldSpace);
} // AddSingleResult
// --------------------------------------------------------------------
}; // myCollision
MaterialRayResult ray_callback(from, to, this);
// If this is a rigid body, m_collision_object is NULL, and the
// rigid body is the actual collision object.
btCollisionWorld::rayTestSingle(trans_from, trans_to,
m_collision_object ? m_collision_object : m_body,
m_collision_shape, world_trans,
ray_callback);
// Get the index of the triangle hit
int index = ray_callback.m_index;
if(ray_callback.hasHit())
{
*xyz = ray_callback.m_hitPointWorld;
*material = m_triangleIndex2Material[index];
if(normal)
{
// If requested interpolate the normal. I.e. instead of using
// the normal of the triangle interpolate the normal at the
// hit position based on the three normals of the triangle.
if(interpolate_normal)
*normal = getInterpolatedNormal(ray_callback.m_index,
ray_callback.m_hitPointWorld);
else
*normal = ray_callback.m_hitNormalWorld;
normal->normalize();
}
}
else
{
*material = NULL;
if(normal)
normal->setValue(0, 1, 0);
}
return ray_callback.hasHit();
} // castRay
/** Casts a ray from 'from' to 'to'. If a triangle of this mesh was hit,
* xyz and material will be set.
* \param from/to The from and to position for the raycast/
* \param xyz The position in world where the ray hit.
* \param material The material of the mesh that was hit.
* \param normal The intrapolated normal at that position.
* \return True if a triangle was hit, false otherwise (and no output
* variable will be set.
*/
bool TriangleMesh::castRay(const btVector3 &from, const btVector3 &to,
btVector3 *xyz, const Material **material,
btVector3 *normal) const
{
if(!m_collision_shape)
{
*material=NULL;
return false;
}
btTransform trans_from;
trans_from.setIdentity();
trans_from.setOrigin(from);
btTransform trans_to;
trans_to.setIdentity();
trans_to.setOrigin(to);
btTransform world_trans;
world_trans.setIdentity();
btCollisionWorld::ClosestRayResultCallback result(from, to);
class MaterialRayResult : public btCollisionWorld::ClosestRayResultCallback
{
public:
const Material* m_material;
const TriangleMesh *m_this;
// --------------------------------------------------------------------
MaterialRayResult(const btVector3 &p1, const btVector3 &p2,
const TriangleMesh *me)
: btCollisionWorld::ClosestRayResultCallback(p1,p2)
{
m_material = NULL;
m_this = me;
} // MaterialRayResult
// --------------------------------------------------------------------
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,
bool normalInWorldSpace)
{
m_material =
m_this->getMaterial(rayResult.m_localShapeInfo->m_triangleIndex);
return btCollisionWorld::ClosestRayResultCallback
::addSingleResult(rayResult, normalInWorldSpace);
} // AddSingleResult
// --------------------------------------------------------------------
}; // myCollision
MaterialRayResult ray_callback(from, to, this);
// If this is a rigid body, m_collision_object is NULL, and the
// rigid body is the actual collision object.
btCollisionWorld::rayTestSingle(trans_from, trans_to,
m_collision_object ? m_collision_object
: m_body,
m_collision_shape, world_trans,
ray_callback);
if(ray_callback.hasHit())
{
*xyz = ray_callback.m_hitPointWorld;
*material = ray_callback.m_material;
if(normal)
{
*normal = ray_callback.m_hitNormalWorld;
normal->normalize();
}
}
else
{
*material = NULL;
if(normal)
normal->setValue(0, 1, 0);
}
return ray_callback.hasHit();
} // castRay
