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