virtual void onContactNotify(NxContactPair& pair, NxU32 events)
{
plPXPhysical* phys1 = (plPXPhysical*)pair.actors[0]->userData;
plPXPhysical* phys2 = (plPXPhysical*)pair.actors[1]->userData;
// Normally, these are always valid because the avatar (who doesn't have
// a physical) will push other physicals away before they actually touch
// his actor. However, if the avatar is warped to a new position he may
// collide with the object for a few frames. We just ignore it.
if (!phys1 || !phys2)
return;
plSimulationMgr::GetInstance()->ConsiderSynch(phys1, phys2);
if (phys1->GetSoundGroup() && phys2->GetSoundGroup())
{
hsPoint3 contactPoint(0, 0, 0);
// Just grab the last contact point
NxContactStreamIterator i(pair.stream);
while (i.goNextPair())
{
while (i.goNextPatch())
{
const NxVec3& contactNormal = i.getPatchNormal();
while (i.goNextPoint())
{
contactPoint = plPXConvert::Point(i.getPoint());
}
}
}
plSimulationMgr::GetInstance()->fSoundMgr->AddContact(
phys1, phys2, contactPoint,
plPXConvert::Vector(pair.sumNormalForce));
}
}
unsigned jfCollisionDetector_x86::boxAndBox(const jfCollisionBox& one,
const jfCollisionBox& two,
jfCollisionData* data
) const
{
//TODO:Ideal candidate for CUDA
jfIntersectionTester_x86 intersectionTester;
// Check for intersection
if (! intersectionTester.boxAndBox(one, two))
{
return 0;
}
jfVector3_x86 oneAxis0, oneAxis1, oneAxis2, oneAxis3;
jfVector3_x86 twoAxis0, twoAxis1, twoAxis2, twoAxis3;
jfVector3_x86 toCentre;
//Find vector between the two centres
one.getAxisVector(3, &oneAxis3);
two.getAxisVector(3, &twoAxis3);
twoAxis3.subtract(oneAxis3, &toCentre);
//Assume no contact at start
jfReal pen = JF_REAL_MAX;
unsigned best = 0xffffff;
// Now we check each axes, returning if it gives us
// a separating axis, and keeping track of the axis with
// the smallest penetration otherwise.
one.getAxisVector(0, &oneAxis0);
CHECK_OVERLAP(oneAxis0, 0);
one.getAxisVector(1, &oneAxis1);
CHECK_OVERLAP(oneAxis1, 1);
one.getAxisVector(2, &oneAxis2);
CHECK_OVERLAP(oneAxis2, 2);
two.getAxisVector(0,&twoAxis0);
CHECK_OVERLAP(twoAxis0, 3);
two.getAxisVector(1,&twoAxis1);
CHECK_OVERLAP(twoAxis1, 4);
two.getAxisVector(2,&twoAxis2);
CHECK_OVERLAP(twoAxis2, 5);
// Store the best axis-major, in case we run into almost
// parallel edge collisions later
unsigned bestSingleAxis = best;
jfVector3_x86 crossProduct;
oneAxis0.crossProduct(twoAxis0, &crossProduct);
CHECK_OVERLAP(crossProduct, 6);
oneAxis0.crossProduct(twoAxis1, &crossProduct);
CHECK_OVERLAP(crossProduct, 7);
oneAxis0.crossProduct(twoAxis2, &crossProduct);
CHECK_OVERLAP(crossProduct, 8);
oneAxis1.crossProduct(twoAxis0, &crossProduct);
CHECK_OVERLAP(crossProduct, 9);
oneAxis1.crossProduct(twoAxis1, &crossProduct);
CHECK_OVERLAP(crossProduct, 10);
oneAxis1.crossProduct(twoAxis2, &crossProduct);
CHECK_OVERLAP(crossProduct, 11);
oneAxis2.crossProduct(twoAxis0, &crossProduct);
CHECK_OVERLAP(crossProduct, 12);
oneAxis2.crossProduct(twoAxis1, &crossProduct);
CHECK_OVERLAP(crossProduct, 13);
oneAxis2.crossProduct(twoAxis2, &crossProduct);
CHECK_OVERLAP(crossProduct, 14);
// Make sure we've got a result.
assert(best != 0xffffff);
// We now know there's a collision, and we know which
// of the axes gave the smallest penetration. We now
// can deal with it in different ways depending on
// the case.
if (best < 3)
{
// We've got a vertex of box two on a face of box one.
fillPointFaceBoxBox(one, two, toCentre, data, best, pen);
/// data->addContacts(1);
return 1;
}
else if (best < 6)
{
// We've got a vertex of box one on a face of box two.
// We use the same algorithm as above, but swap around
// one and two (and therefore also the vector between their
// centres).
toCentre *= -1;
fillPointFaceBoxBox(two, one, toCentre, data, (best-3), pen);
/// data->addContacts(1);
return 1;
}
else
{
// We've got an edge-edge contact. Find out which axes
best -= 6;
unsigned oneAxisIndex = best / 3;
unsigned twoAxisIndex = best % 3;
jfVector3_x86 oneAxis;
one.getAxisVector(oneAxisIndex, &oneAxis);
jfVector3_x86 twoAxis;
two.getAxisVector(twoAxisIndex, &twoAxis);
jfVector3_x86 axis;
oneAxis.crossProduct(twoAxis, &axis);
axis.normalize();
// The axis should point from box one to box two.
if (axis.dotProduct(toCentre) > 0)
{
axis *= -1.0f;
}
// We have the axes, but not the edges: each axis has 4 edges parallel
// to it, we need to find which of the 4 for each object. We do
// that by finding the point in the centre of the edge. We know
// its component in the direction of the box's collision axis is zero
// (its a mid-point) and we determine which of the extremes in each
// of the other axes is closest.
jfVector3_x86 ptOnOneEdge;
one.getHalfSize(&ptOnOneEdge);
jfVector3_x86 ptOnTwoEdge;
two.getHalfSize(&ptOnTwoEdge);
for (unsigned i = 0; i < 3; i++)
{
jfVector3_x86 oneAxisCurrent;
jfVector3_x86 twoAxisCurrent;
one.getAxisVector(i, &oneAxisCurrent);
two.getAxisVector(i, &twoAxisCurrent);
if (i == oneAxisIndex)
{
ptOnOneEdge.setElem(i,0);
}
else if (oneAxisCurrent.dotProduct(axis) > 0)
{
ptOnOneEdge.setElem(i, -ptOnOneEdge.getElem(i));
}
if (i == twoAxisIndex)
{
ptOnTwoEdge.setElem(i, 0);
}
else if (twoAxisCurrent.dotProduct(axis) < 0)
{
ptOnTwoEdge.setElem(i, -ptOnTwoEdge.getElem(i));
}
}
// Move them into world coordinates (they are already oriented
// correctly, since they have been derived from the axes).
jfMatrix4_x86 oneTransform, twoTransform;
one.getTransformMatrix(&oneTransform);
oneTransform.multiply(ptOnOneEdge, &ptOnOneEdge);
two.getTransformMatrix(&twoTransform);
twoTransform.multiply(ptOnTwoEdge, &ptOnTwoEdge);
// So we have a point and a direction for the colliding edges.
// We need to find out point of closest approach of the two
// line-segments.
jfVector3_x86 vertex;
jfVector3_x86 oneHalfSize, twoHalfSize;
one.getHalfSize(&oneHalfSize);
two.getHalfSize(&twoHalfSize);
contactPoint(
ptOnOneEdge,
oneAxis,
oneHalfSize.getElem(oneAxisIndex),
ptOnTwoEdge,
twoAxis,
twoHalfSize.getElem(twoAxisIndex),
(bestSingleAxis > 2),
&vertex
);
// We can fill the contact.
//jfContact* contact = data->contacts;
jfContact_x86 contact;
contact.setPenetration(pen);
contact.setContactNormal(axis);
contact.setContactPoint(vertex);
contact.setBodyData(one.getBody(),
two.getBody(),
data->getFriction(),
data->getRestitution());
data->addContact(contact);
return 1;
}
return 0;
}
//load initial path
// file has following form (like a bvh)
// HIERARCHY
// Ordered list of all joints for which there are constraints
// MOTION
// Frames: 16
// Frame Time: 1
// "X Y Z Rx Ry Rz J1 J2... Jn" one line per state
// "CONTACT EFFECTORS N" N is the number of effectors
// "hasContact X Y Z qx qy qz qw" hasContact 0 or 1, times number effectors one line per state
std::vector<std::string> InitTrajectoryFromFile(std::vector<Eigen::VectorXd>& waypoints, std::vector<Eigen::MatrixXd>& contactPoints, const std::string& filepath)
{
std::vector<std::string> res;
bool hierarchy = false;
bool motion = false;
bool contacts = false;
double offset [3] = {0,0,0};
int nbEffectors = 0;
std::ifstream myfile (filepath.c_str());
if (myfile.is_open())
{
std::string line;
while (myfile.good())
{
getline(myfile, line);
if(line.find("HIERARCHY") != std::string::npos)
{
hierarchy = true;
}
else if(line.find("OFFSET ") != std::string::npos)
{
hierarchy = false;
line = line.substr(7);
char *endptr;
int h = 0;
offset[h++] = strtod(line.c_str(), &endptr);
for(; h< 3; ++h)
{
double tg = strtod(endptr, &endptr);
offset[h] = tg; // strtod(endptr, &endptr);
}
}
else if(line.find("MOTION") != std::string::npos)
{
hierarchy = false;
}
else if(line.find("Frame Time") != std::string::npos)
{
motion = true;
}
else if(line.find("CONTACT EFFECTORS ") != std::string::npos)
{
motion = false;
contacts = true;
line = line.substr(18);
char *endptr;
nbEffectors = (int)strtod(line.c_str(), &endptr);
}
else if(!line.empty())
{
if(hierarchy)
{
res.push_back(line);
}
else if(motion)
{
Eigen::VectorXd waypoint(res.size());
char *endptr;
unsigned int h = 0;
waypoint[h++] = strtod(line.c_str(), &endptr);
for(; h< res.size(); ++h)
{
waypoint[h] = strtod(endptr, &endptr);
}
for(int i =0; i<3; ++i)
{
waypoint[i] += offset[i];
}
waypoint[2] -= 1;
waypoints.push_back(waypoint);
}
else if(contacts)
{
Eigen::MatrixXd contactPoint(nbEffectors, 8);
char *endptr;
for(int i=0; i< nbEffectors; ++i)
{
int h = 0;
if (i == 0)
contactPoint(i,h++) = strtod(line.c_str(), &endptr);
else
contactPoint(i,h++) = strtod(endptr, &endptr);
for(; h< 8; ++h)
{
contactPoint(i,h) = strtod(endptr, &endptr);
}
for(int k =1; k<4; ++k)
{
contactPoint(i,k) += offset[k];
}
contactPoint(i,3) -= 1;
}
contactPoints.push_back(contactPoint);
}
}
}
myfile.close();
}
else
{
std::cout << "can not find initial path file" << filepath << std::endl;
}
if(!(contactPoints.empty() || waypoints.size() == contactPoints.size()))
{
std::cout << "Error in initial path file: not same number of contacts and frames" << filepath << std::endl;
}
return res;
}
void ContactGenerator::box_box(const CollisionShape &a, const RigidBody &rbA, const CollisionShape &b, const RigidBody &rbB, ContactManifold &contactManifold)
{
// Make sure we have contacts left and Check if they're boxes.
if ( a.getShapeType() != SHAPE_BOX || b.getShapeType() != SHAPE_BOX ) { return; }
// then typecast their appropriate shapes
const ShapeBox& boxA = (const ShapeBox&)a;
const ShapeBox& boxB = (const ShapeBox&)b;
const Vec3& boxAHalfExtents = boxA.getHalfExtents();
const Vec3& boxBHalfExtents = boxB.getHalfExtents();
Transform boxATransform = rbA.getTransform() * a.getOffset();
Transform boxBTransform = rbB.getTransform() * b.getOffset();
// Vector between box centres.
Vec3 separation = boxBTransform.getPosition() - boxATransform.getPosition();
Scalar smallestPen = SCALAR_MAX;
unsigned int bestPen = 0xffffffff;
// Check each axis, keeping track of the axis with the smallest penetration.
// Stops when if finds an axis without penetration.
if (!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0), separation, 0, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1), separation, 1, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2), separation, 2, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(0), separation, 3, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(1), separation, 4, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(2), separation, 5, smallestPen, bestPen) )
{
return;
}
unsigned bestSingleAxis = bestPen;
if (!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(0)), separation, 6, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(1)), separation, 7, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(2)), separation, 8, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(0)), separation, 9, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(1)), separation, 10, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(2)), separation, 11, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(0)), separation, 12, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(1)), separation, 13, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(2)), separation, 14, smallestPen, bestPen) )
{
return;
}
// We've found a collision, and we know which of the axes gave the smallest penetration.
if (bestPen < 3)
{
// Vertex of boxB in face of boxA
// TODO: Do stuff
//std::cout << bestPen << " Vertex of boxB in face of boxA\n";
fillPointFaceBoxBox(boxA, boxATransform, boxB, boxBTransform, separation, contactManifold, bestPen, smallestPen, false);
return;
}
else if (bestPen < 6)
{
// Vertex of boxA in face of boxB
// TODO: Do stuff
//std::cout << bestPen << " Vertex of boxA in face of boxB\n";
fillPointFaceBoxBox(boxA, boxATransform, boxB, boxBTransform, -separation, contactManifold, bestPen-3, smallestPen, true);
return;
}
else
{
// Edge Edge contact.
//std::cout << "Colliding >= 6";
//std::cout << bestPen << " Edge-Edge\n";
// Find which axis.
bestPen -= 6;
unsigned int axisIndexA = bestPen / 3;
unsigned int axisIndexB = bestPen % 3;
Vec3 axisA = boxATransform.getAxisVector(axisIndexA);
Vec3 axisB = boxBTransform.getAxisVector(axisIndexB);
Vec3 axis = axisA.cross(axisB);
axis.normalize();
// Axis should point from box one to box two.
if ( axis.dot(separation) > 0 )
{
axis = -axis;
}
Vec3 ptOnEdgeA = boxAHalfExtents;
Vec3 ptOnEdgeB = -boxBHalfExtents;
for (unsigned int i = 0; i < 3; i++)
{
if (i == axisIndexA)
{
ptOnEdgeA[i] = 0;
}
else if (boxATransform.getAxisVector(i).dot(axis) > 0)
{
ptOnEdgeA[i] = -ptOnEdgeA[i];
}
if (i == axisIndexB)
{
ptOnEdgeB[i] = 0;
}
else if (boxBTransform.getAxisVector(i).dot(axis) > 0)
{
ptOnEdgeB[i] = -ptOnEdgeB[i];
}
}
// Transform the points into world coordinates.
ptOnEdgeA = boxATransform * ptOnEdgeA;
ptOnEdgeB = boxBTransform * ptOnEdgeB;
// Find the point of closest approach of the two
// line-segments.
Vec3 vertex = contactPoint(ptOnEdgeA, axisA, boxAHalfExtents.get(axisIndexA), ptOnEdgeB, axisB, boxBHalfExtents.get(axisIndexB), bestSingleAxis > 2);
// Create contact data
ContactPoint newContact;
newContact.normal = axis;
newContact.penetration = smallestPen;
newContact.position = vertex;
contactManifold.addContactPoint(newContact);
}
return;
}
unsigned CollisionDetector::boxAndBox(
const CollisionBox &one,
const CollisionBox &two,
CollisionData *data
)
{
//if (!IntersectionTests::boxAndBox(one, two)) return 0;
// Find the vector between the two centres
Vector3 toCentre = two.getAxis(3) - one.getAxis(3);
// We start assuming there is no contact
real pen = REAL_MAX;
unsigned best = 0xffffff;
// Now we check each axes, returning if it gives us
// a separating axis, and keeping track of the axis with
// the smallest penetration otherwise.
CHECK_OVERLAP(one.getAxis(0), 0);
CHECK_OVERLAP(one.getAxis(1), 1);
CHECK_OVERLAP(one.getAxis(2), 2);
CHECK_OVERLAP(two.getAxis(0), 3);
CHECK_OVERLAP(two.getAxis(1), 4);
CHECK_OVERLAP(two.getAxis(2), 5);
// Store the best axis-major, in case we run into almost
// parallel edge collisions later
unsigned bestSingleAxis = best;
CHECK_OVERLAP(one.getAxis(0) % two.getAxis(0), 6);
CHECK_OVERLAP(one.getAxis(0) % two.getAxis(1), 7);
CHECK_OVERLAP(one.getAxis(0) % two.getAxis(2), 8);
CHECK_OVERLAP(one.getAxis(1) % two.getAxis(0), 9);
CHECK_OVERLAP(one.getAxis(1) % two.getAxis(1), 10);
CHECK_OVERLAP(one.getAxis(1) % two.getAxis(2), 11);
CHECK_OVERLAP(one.getAxis(2) % two.getAxis(0), 12);
CHECK_OVERLAP(one.getAxis(2) % two.getAxis(1), 13);
CHECK_OVERLAP(one.getAxis(2) % two.getAxis(2), 14);
// Make sure we've got a result.
assert(best != 0xffffff);
// We now know there's a collision, and we know which
// of the axes gave the smallest penetration. We now
// can deal with it in different ways depending on
// the case.
if (best < 3)
{
// We've got a vertex of box two on a face of box one.
fillPointFaceBoxBox(one, two, toCentre, data, best, pen);
data->addContacts(1);
return 1;
}
else if (best < 6)
{
// We've got a vertex of box one on a face of box two.
// We use the same algorithm as above, but swap around
// one and two (and therefore also the vector between their
// centres).
fillPointFaceBoxBox(two, one, toCentre*-1.0f, data, best-3, pen);
data->addContacts(1);
return 1;
}
else
{
// We've got an edge-edge contact. Find out which axes
best -= 6;
unsigned oneAxisIndex = best / 3;
unsigned twoAxisIndex = best % 3;
Vector3 oneAxis = one.getAxis(oneAxisIndex);
Vector3 twoAxis = two.getAxis(twoAxisIndex);
Vector3 axis = oneAxis % twoAxis;
axis.normalise();
// The axis should point from box one to box two.
if (axis * toCentre > 0) axis = axis * -1.0f;
// We have the axes, but not the edges: each axis has 4 edges parallel
// to it, we need to find which of the 4 for each object. We do
// that by finding the point in the centre of the edge. We know
// its component in the direction of the box's collision axis is zero
// (its a mid-point) and we determine which of the extremes in each
// of the other axes is closest.
Vector3 ptOnOneEdge = one.halfSize;
Vector3 ptOnTwoEdge = two.halfSize;
for (unsigned i = 0; i < 3; i++)
{
if (i == oneAxisIndex) ptOnOneEdge[i] = 0;
else if (one.getAxis(i) * axis > 0) ptOnOneEdge[i] = -ptOnOneEdge[i];
if (i == twoAxisIndex) ptOnTwoEdge[i] = 0;
else if (two.getAxis(i) * axis < 0) ptOnTwoEdge[i] = -ptOnTwoEdge[i];
}
// Move them into world coordinates (they are already oriented
// correctly, since they have been derived from the axes).
ptOnOneEdge = one.transform * ptOnOneEdge;
ptOnTwoEdge = two.transform * ptOnTwoEdge;
// So we have a point and a direction for the colliding edges.
// We need to find out point of closest approach of the two
// line-segments.
Vector3 vertex = contactPoint(
ptOnOneEdge, oneAxis, one.halfSize[oneAxisIndex],
ptOnTwoEdge, twoAxis, two.halfSize[twoAxisIndex],
bestSingleAxis > 2
);
// We can fill the contact.
Contact* contact = data->contacts;
contact->penetration = pen;
contact->contactNormal = axis;
contact->contactPoint = vertex;
contact->setBodyData(one.body, two.body,
data->friction, data->restitution);
data->addContacts(1);
return 1;
}
return 0;
}
