virtual btScalar addSingleResult(btManifoldPoint& cp,
const btCollisionObject* col0, int partId0, int index0,
const btCollisionObject* col1, int partId1, int index1)
{
const RigidBody* body = dynamic_cast<const RigidBody*>(col1);
if(body && body->mName != mFilter)
{
btScalar distsqr = mOrigin.distance2(cp.getPositionWorldOnA());
if(!mObject || distsqr < mLeastDistSqr)
{
mObject = body;
mLeastDistSqr = distsqr;
mContactPoint = cp.getPositionWorldOnA();
}
}
return 0.f;
}
// -----------------------------------------------------------------------------
void IrrDebugDrawer::drawLine(const btVector3& from, const btVector3& to,
const btVector3& color)
{
video::SColor c(255, (int)(color.getX()*255), (int)(color.getY()*255),
(int)(color.getZ()*255) );
//World::getWorld()->getCa
if (from.distance2(m_camera_pos) > 10000) return;
std::vector<float>& v = m_lines[c];
v.push_back(from.getX());
v.push_back(from.getY());
v.push_back(from.getZ());
v.push_back(to.getX());
v.push_back(to.getY());
v.push_back(to.getZ());
//draw3DLine((const core::vector3df&)from, (const core::vector3df&)to, c);
}
btScalar getObjectMaximumGravityTorqueLength(const btCollisionShape &object_shape)
{
// Calculate the maximum acceleration that can happen for a particular object under gravity
std::vector<btVector3> object_corner_point_list;
object_corner_point_list.reserve(150);
// get all points in the compound object
if (object_shape.isCompound())
{
const btCompoundShape * object_compound_shape = (btCompoundShape *)&object_shape;
for (int i = 0; i < object_compound_shape->getNumChildShapes(); i++)
{
const btCollisionShape * child_shape = object_compound_shape->getChildShape(i);
const btTransform child_transform = object_compound_shape->getChildTransform(i);
if (child_shape->isConvex())
{
const btConvexHullShape * child_hull_shape = (btConvexHullShape *)child_shape;
for (int pts = 0; pts < child_hull_shape->getNumPoints(); pts++)
{
btVector3 corner_points = child_hull_shape->getScaledPoint(pts);
object_corner_point_list.push_back(child_transform * corner_points);
}
}
}
}
// get the farthest distance from the object center of gravity
// const btVector3 cog = object.getCenterOfMassPosition();
const btVector3 cog(0.,0.,0.);
btScalar max_dist_squared = 0;
for (std::vector<btVector3>::iterator it = object_corner_point_list.begin(); it!= object_corner_point_list.end(); ++it)
{
btScalar point_distance_to_cog = cog.distance2(*it);
max_dist_squared = (point_distance_to_cog > max_dist_squared) ? point_distance_to_cog : max_dist_squared;
}
return sqrt(max_dist_squared);
}
static bool IsEqual(const btVector3 &pt0, const btVector3 &pt1) {
return pt0.distance2(pt1) < 1e-8f;
}
// clips an obstacle shape if it is within range of the sensors
// if not returns false
bool cSpace::clipShape(btVector3 cc, QList<btVector3>& ls)
{
if(ls.size() < 2) return false;
int j=0;
cc.setZ(0);
// check if the whole obstacle is in range
do{
if(cc.distance2(ls[j]) > m_detectRangeSq) break; // check if the point is in-range
j++;
}while(j<ls.size());
if(j == ls.size()) return true; // the whole obstacle is in range
int i;
int nexti;
int arcType;
btVector3 xpoint1,xpoint2;
bool outofrange = true;
i=0;
while(i<ls.size())
{
if(i == ls.size()-1) nexti = 0;
else nexti = i+1;
arcType = arcIntersection(cc, m_detectRange, ls[i], ls[nexti], &xpoint1, &xpoint2);
if(arcType == 0) // NO intersection
i++;
else if(arcType == 1 || arcType == 2){ // ONE intersection
if(arcType == 2) xpoint1 = xpoint2;
if(nexti == 0) ls.append(xpoint1); // append new point ls if at the wraparound point
else ls.insert(nexti,xpoint1); // insert the intersection point to the ls
i+=2;
outofrange = false;
}
else{ // TWO intersections
if(nexti == 0){ // add the two intersect points between i and nexti
if(ls[i].distance2(xpoint1) < ls[i].distance2(xpoint2)){ // append them in order
ls << xpoint1;
ls << xpoint2;
}
else{
ls << xpoint2;
ls << xpoint1;
}
}
else{
if(ls[i].distance2(xpoint1) < ls[i].distance2(xpoint2)){ // insert them in order
ls.insert(nexti,xpoint1);
ls.insert(nexti+1,xpoint2);
}
else{
ls.insert(nexti,xpoint2);
ls.insert(nexti+1,xpoint1);
}
}
i+=3;
outofrange = false;
}
}
if(outofrange) return false; // the entire obstacle is out of range
// remove any points outside of the sensor range
i=0;
while(i<ls.size()){
if(cc.distance2(ls[i]) > m_detectRangeSq+0.001)
ls.removeAt(i);
else
i++;
}
if(ls.size() == 2){
btVector3 cx;
cx = (ls[1] - ls[0]).cross((ls[0]-btVector3(0,0,1)) - ls[0]);
cx.normalize();
ls << (ls[1]+(cx*0.1));
ls << (ls[0]+(cx*0.1));
}
return true;
}