Add capsule/capsule and capsule/sphere collision algorithm
This commit is contained in:
parent
e9f2f94f64
commit
30e0132e15
|
@ -78,6 +78,8 @@ SET (REACTPHYSICS3D_SOURCES
|
||||||
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
|
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
|
||||||
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h"
|
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h"
|
||||||
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
|
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
|
||||||
|
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.h"
|
||||||
|
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
|
||||||
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
|
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
|
||||||
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
|
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
|
||||||
"src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h"
|
"src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h"
|
||||||
|
|
|
@ -30,8 +30,122 @@
|
||||||
// We want to use the ReactPhysics3D namespace
|
// We want to use the ReactPhysics3D namespace
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
|
bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
|
||||||
ContactPointInfo& contactPointInfo) {
|
|
||||||
|
|
||||||
|
const decimal parallelEpsilon = decimal(0.001);
|
||||||
|
|
||||||
|
// Get the capsule collision shapes
|
||||||
|
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
|
||||||
|
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
|
||||||
|
|
||||||
|
// Get the transform from capsule 1 local-space to capsule 2 local-space
|
||||||
|
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
|
||||||
|
|
||||||
|
// Compute the end-points of the inner segment of the first capsule
|
||||||
|
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
|
||||||
|
Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
|
||||||
|
capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
|
||||||
|
capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
|
||||||
|
|
||||||
|
// Compute the end-points of the inner segment of the second capsule
|
||||||
|
const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
|
||||||
|
const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
|
||||||
|
|
||||||
|
// The two inner capsule segments
|
||||||
|
const Vector3 seg1 = capsule1SegB - capsule1SegA;
|
||||||
|
const Vector3 seg2 = capsule2SegB - capsule2SegA;
|
||||||
|
|
||||||
|
// Compute the sum of the radius of the two capsules (virtual spheres)
|
||||||
|
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
|
||||||
|
|
||||||
|
// If the two capsules are parallel (we create two contact points)
|
||||||
|
if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) {
|
||||||
|
|
||||||
|
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
|
||||||
|
const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
|
||||||
|
if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) {
|
||||||
|
|
||||||
|
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
|
||||||
|
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the planes that goes through the extrem points of the inner segment of capsule 1
|
||||||
|
decimal d1 = seg1.dot(capsule1SegA);
|
||||||
|
decimal d2 = -seg1.dot(capsule1SegB);
|
||||||
|
|
||||||
|
// Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
|
||||||
|
// segment of capsule 1
|
||||||
|
decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
|
||||||
|
decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
|
||||||
|
|
||||||
|
bool isClipValid = false; // True if the segments were overlapping (the clip segment is valid)
|
||||||
|
|
||||||
|
// Clip the inner segment of capsule 2
|
||||||
|
Vector3 clipPointA, clipPointB;
|
||||||
|
if (t1 >= decimal(0.0)) {
|
||||||
|
|
||||||
|
if (t1 > decimal(1.0)) t1 = decimal(1.0);
|
||||||
|
clipPointA = capsule2SegB - t1 * seg2;
|
||||||
|
isClipValid = true;
|
||||||
|
}
|
||||||
|
if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
|
||||||
|
|
||||||
|
if (t2 > decimal(1.0)) t2 = decimal(1.0);
|
||||||
|
clipPointB = capsule2SegA + t2 * seg2;
|
||||||
|
isClipValid = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we have a valid clip segment
|
||||||
|
if (isClipValid) {
|
||||||
|
|
||||||
|
Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA);
|
||||||
|
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
|
||||||
|
|
||||||
|
const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
|
||||||
|
const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
|
||||||
|
const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
|
||||||
|
const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
|
||||||
|
|
||||||
|
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
|
||||||
|
|
||||||
|
decimal penetrationDepth = sumRadius - segmentsDistance;
|
||||||
|
|
||||||
|
// Create the contact info object
|
||||||
|
// TODO : Make sure we create two contact points at the same time (same method here)
|
||||||
|
contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local);
|
||||||
|
contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the closest points between the two inner capsule segments
|
||||||
|
Vector3 closestPointCapsule1Seg;
|
||||||
|
Vector3 closestPointCapsule2Seg;
|
||||||
|
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
|
||||||
|
closestPointCapsule1Seg, closestPointCapsule2Seg);
|
||||||
|
|
||||||
|
// Compute the distance between the sphere center and the closest point on the segment
|
||||||
|
Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
|
||||||
|
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
|
||||||
|
|
||||||
|
// If the collision shapes overlap
|
||||||
|
if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
|
||||||
|
|
||||||
|
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
|
||||||
|
closestPointsSeg1ToSeg2 /= closestPointsDistance;
|
||||||
|
|
||||||
|
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
|
||||||
|
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
|
||||||
|
|
||||||
|
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
|
||||||
|
|
||||||
|
decimal penetrationDepth = sumRadius - closestPointsDistance;
|
||||||
|
|
||||||
|
// Create the contact info object
|
||||||
|
contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,7 +38,7 @@ namespace reactphysics3d {
|
||||||
// Class CapsuleVsCapsuleAlgorithm
|
// Class CapsuleVsCapsuleAlgorithm
|
||||||
/**
|
/**
|
||||||
* This class is used to compute the narrow-phase collision detection
|
* This class is used to compute the narrow-phase collision detection
|
||||||
* between two capsule collision shapes.
|
* between two capsules collision shapes.
|
||||||
*/
|
*/
|
||||||
class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
|
class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
|
||||||
|
|
||||||
|
|
|
@ -26,6 +26,7 @@
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "mathematics_functions.h"
|
#include "mathematics_functions.h"
|
||||||
#include "Vector3.h"
|
#include "Vector3.h"
|
||||||
|
#include <cassert>
|
||||||
|
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
|
@ -50,10 +51,152 @@ void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, c
|
||||||
u = decimal(1.0) - v - w;
|
u = decimal(1.0) - v - w;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Clamp a vector such that it is no longer than a given maximum length
|
/// Clamp a vector such that it is no longer than a given maximum length
|
||||||
Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
|
Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
|
||||||
if (vector.lengthSquare() > maxLength * maxLength) {
|
if (vector.lengthSquare() > maxLength * maxLength) {
|
||||||
return vector.getUnit() * maxLength;
|
return vector.getUnit() * maxLength;
|
||||||
}
|
}
|
||||||
return vector;
|
return vector;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
|
||||||
|
Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) {
|
||||||
|
|
||||||
|
const Vector3 ab = segPointB - segPointA;
|
||||||
|
|
||||||
|
decimal abLengthSquare = ab.lengthSquare();
|
||||||
|
|
||||||
|
// If the segment has almost zero length
|
||||||
|
if (abLengthSquare < MACHINE_EPSILON) {
|
||||||
|
|
||||||
|
// Return one end-point of the segment as the closest point
|
||||||
|
return segPointA;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Project point C onto "AB" line
|
||||||
|
decimal t = (pointC - segPointA).dot(ab) / abLengthSquare;
|
||||||
|
|
||||||
|
// If projected point onto the line is outside the segment, clamp it to the segment
|
||||||
|
if (t < decimal(0.0)) t = decimal(0.0);
|
||||||
|
if (t > decimal(1.0)) t = decimal(1.0);
|
||||||
|
|
||||||
|
// Return the closest point on the segment
|
||||||
|
return segPointA + t * ab;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the closest points between two segments
|
||||||
|
/// This method uses the technique described in the book Real-Time
|
||||||
|
/// collision detection by Christer Ericson.
|
||||||
|
void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
|
||||||
|
const Vector3& seg2PointA, const Vector3& seg2PointB,
|
||||||
|
Vector3& closestPointSeg1, Vector3& closestPointSeg2) {
|
||||||
|
|
||||||
|
const Vector3 d1 = seg1PointB - seg1PointA;
|
||||||
|
const Vector3 d2 = seg2PointB - seg2PointA;
|
||||||
|
const Vector3 r = seg1PointA - seg2PointA;
|
||||||
|
decimal a = d1.lengthSquare();
|
||||||
|
decimal e = d2.lengthSquare();
|
||||||
|
decimal f = d2.dot(r);
|
||||||
|
decimal s, t;
|
||||||
|
|
||||||
|
// If both segments degenerate into points
|
||||||
|
if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) {
|
||||||
|
|
||||||
|
closestPointSeg1 = seg1PointA;
|
||||||
|
closestPointSeg2 = seg2PointA;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point
|
||||||
|
|
||||||
|
s = decimal(0.0);
|
||||||
|
|
||||||
|
// Compute the closest point on second segment
|
||||||
|
t = clamp(f / e, decimal(0.0), decimal(1.0));
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
decimal c = d1.dot(r);
|
||||||
|
|
||||||
|
// If the second segment degenerates into a point
|
||||||
|
if (e <= MACHINE_EPSILON) {
|
||||||
|
|
||||||
|
t = decimal(0.0);
|
||||||
|
s = clamp(-c / a, decimal(0.0), decimal(1.0));
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
decimal b = d1.dot(d2);
|
||||||
|
decimal denom = a * e - b * b;
|
||||||
|
|
||||||
|
// If the segments are not parallel
|
||||||
|
if (denom != decimal(0.0)) {
|
||||||
|
|
||||||
|
// Compute the closest point on line 1 to line 2 and
|
||||||
|
// clamp to first segment.
|
||||||
|
s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0));
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
// Pick an arbitrary point on first segment
|
||||||
|
s = decimal(0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the point on line 2 closest to the closest point
|
||||||
|
// we have just found
|
||||||
|
t = (b * s + f) / e;
|
||||||
|
|
||||||
|
// If this closest point is inside second segment (t in [0, 1]), we are done.
|
||||||
|
// Otherwise, we clamp the point to the second segment and compute again the
|
||||||
|
// closest point on segment 1
|
||||||
|
if (t < decimal(0.0)) {
|
||||||
|
t = decimal(0.0);
|
||||||
|
s = clamp(-c / a, decimal(0.0), decimal(1.0));
|
||||||
|
}
|
||||||
|
else if (t > decimal(1.0)) {
|
||||||
|
t = decimal(1.0);
|
||||||
|
s = clamp((b - c) / a, decimal(0.0), decimal(1.0));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the closest points on both segments
|
||||||
|
closestPointSeg1 = seg1PointA + d1 * s;
|
||||||
|
closestPointSeg2 = seg2PointA + d2 * t;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the intersection between a plane and a segment
|
||||||
|
// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method
|
||||||
|
// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such
|
||||||
|
// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise,
|
||||||
|
// there is no intersection between the plane and the segment.
|
||||||
|
decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) {
|
||||||
|
|
||||||
|
const decimal parallelEpsilon = decimal(0.0001);
|
||||||
|
decimal t = decimal(-1);
|
||||||
|
|
||||||
|
// Segment AB
|
||||||
|
const Vector3 ab = segB - segA;
|
||||||
|
|
||||||
|
decimal nDotAB = planeNormal.dot(ab);
|
||||||
|
|
||||||
|
// If the segment is not parallel to the plane
|
||||||
|
if (nDotAB > parallelEpsilon) {
|
||||||
|
t = (planeD - planeNormal.dot(segA)) / nDotAB;
|
||||||
|
}
|
||||||
|
|
||||||
|
return t;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB"
|
||||||
|
decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
|
||||||
|
|
||||||
|
decimal distAB = (linePointB - linePointA).length();
|
||||||
|
|
||||||
|
if (distAB < MACHINE_EPSILON) {
|
||||||
|
return (point - linePointA).length();
|
||||||
|
}
|
||||||
|
|
||||||
|
return ((point - linePointA).cross(point - linePointB)).length() / distAB;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -78,10 +78,26 @@ inline bool sameSign(decimal a, decimal b) {
|
||||||
/// Clamp a vector such that it is no longer than a given maximum length
|
/// Clamp a vector such that it is no longer than a given maximum length
|
||||||
Vector3 clamp(const Vector3& vector, decimal maxLength);
|
Vector3 clamp(const Vector3& vector, decimal maxLength);
|
||||||
|
|
||||||
|
// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
|
||||||
|
Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC);
|
||||||
|
|
||||||
|
// Compute the closest points between two segments
|
||||||
|
void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
|
||||||
|
const Vector3& seg2PointA, const Vector3& seg2PointB,
|
||||||
|
Vector3& closestPointSeg1, Vector3& closestPointSeg2);
|
||||||
|
|
||||||
/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
|
/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
|
||||||
void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
|
void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
|
||||||
const Vector3& p, decimal& u, decimal& v, decimal& w);
|
const Vector3& p, decimal& u, decimal& v, decimal& w);
|
||||||
|
|
||||||
|
/// Compute the intersection between a plane and a segment
|
||||||
|
decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal);
|
||||||
|
|
||||||
|
/// Compute the distance between a point and a line
|
||||||
|
decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -124,6 +124,52 @@ class TestMathematicsFunctions : public Test {
|
||||||
|
|
||||||
computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w);
|
computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w);
|
||||||
test(approxEqual(u + v + w, 1.0, 0.000001));
|
test(approxEqual(u + v + w, 1.0, 0.000001));
|
||||||
|
|
||||||
|
// Test computeClosestPointBetweenTwoSegments()
|
||||||
|
Vector3 closestSeg1, closestSeg2;
|
||||||
|
computeClosestPointBetweenTwoSegments(Vector3(4, 0, 0), Vector3(6, 0, 0), Vector3(8, 0, 0), Vector3(8, 6, 0), closestSeg1, closestSeg2);
|
||||||
|
test(approxEqual(closestSeg1.x, 6.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.y, 0.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.z, 0.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.x, 8.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.y, 0.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.z, 0.0, 0.000001));
|
||||||
|
computeClosestPointBetweenTwoSegments(Vector3(4, 6, 5), Vector3(4, 6, 5), Vector3(8, 3, -9), Vector3(8, 3, -9), closestSeg1, closestSeg2);
|
||||||
|
test(approxEqual(closestSeg1.x, 4.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.y, 6.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.z, 5.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.x, 8.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.y, 3.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.z, -9.0, 0.000001));
|
||||||
|
computeClosestPointBetweenTwoSegments(Vector3(0, -5, 0), Vector3(0, 8, 0), Vector3(6, 3, 0), Vector3(10, -3, 0), closestSeg1, closestSeg2);
|
||||||
|
test(approxEqual(closestSeg1.x, 0.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.y, 3.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.z, 0.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.x, 6.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.y, 3.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.z, 0.0, 0.000001));
|
||||||
|
computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2);
|
||||||
|
test(approxEqual(closestSeg1.x, 1.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.y, 5.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg1.z, -5.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.x, 1.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.y, 5.0, 0.000001));
|
||||||
|
test(approxEqual(closestSeg2.z, -5.0, 0.000001));
|
||||||
|
|
||||||
|
// Test computePlaneSegmentIntersection();
|
||||||
|
test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001));
|
||||||
|
test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001));
|
||||||
|
test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001));
|
||||||
|
test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001));
|
||||||
|
decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0));
|
||||||
|
test(tIntersect < 0.0 && tIntersect > 1.0);
|
||||||
|
|
||||||
|
// Test computeDistancePointToLineDistance()
|
||||||
|
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001));
|
||||||
|
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001));
|
||||||
|
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001));
|
||||||
|
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001));
|
||||||
|
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001));
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -71,6 +71,17 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
|
||||||
mSphere2->setColor(mGreyColorDemo);
|
mSphere2->setColor(mGreyColorDemo);
|
||||||
mSphere2->setSleepingColor(mRedColorDemo);
|
mSphere2->setSleepingColor(mRedColorDemo);
|
||||||
|
|
||||||
|
// ---------- Capsule 1 ---------- //
|
||||||
|
openglframework::Vector3 position3(4, 0, 0);
|
||||||
|
|
||||||
|
// Create a cylinder and a corresponding collision body in the dynamics world
|
||||||
|
mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath);
|
||||||
|
mAllShapes.push_back(mCapsule1);
|
||||||
|
|
||||||
|
// Set the color
|
||||||
|
mCapsule1->setColor(mGreyColorDemo);
|
||||||
|
mCapsule1->setSleepingColor(mRedColorDemo);
|
||||||
|
|
||||||
// ---------- Cone ---------- //
|
// ---------- Cone ---------- //
|
||||||
//openglframework::Vector3 position4(0, 0, 0);
|
//openglframework::Vector3 position4(0, 0, 0);
|
||||||
|
|
||||||
|
@ -93,16 +104,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
|
||||||
//mCylinder->setColor(mGreyColorDemo);
|
//mCylinder->setColor(mGreyColorDemo);
|
||||||
//mCylinder->setSleepingColor(mRedColorDemo);
|
//mCylinder->setSleepingColor(mRedColorDemo);
|
||||||
|
|
||||||
// ---------- Capsule ---------- //
|
|
||||||
//openglframework::Vector3 position6(0, 0, 0);
|
|
||||||
|
|
||||||
// Create a cylinder and a corresponding collision body in the dynamics world
|
|
||||||
//mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 ,
|
|
||||||
// mCollisionWorld, mMeshFolderPath);
|
|
||||||
|
|
||||||
// Set the color
|
|
||||||
//mCapsule->setColor(mGreyColorDemo);
|
|
||||||
//mCapsule->setSleepingColor(mRedColorDemo);
|
|
||||||
|
|
||||||
// ---------- Convex Mesh ---------- //
|
// ---------- Convex Mesh ---------- //
|
||||||
//openglframework::Vector3 position7(0, 0, 0);
|
//openglframework::Vector3 position7(0, 0, 0);
|
||||||
|
@ -135,7 +136,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
|
||||||
//mHeightField->setSleepingColor(mRedColorDemo);
|
//mHeightField->setSleepingColor(mRedColorDemo);
|
||||||
|
|
||||||
// Create the VBO and VAO to render the lines
|
// Create the VBO and VAO to render the lines
|
||||||
createVBOAndVAO(mPhongShader);
|
//createVBOAndVAO(mPhongShader);
|
||||||
|
|
||||||
mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
|
mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
|
||||||
}
|
}
|
||||||
|
@ -283,6 +284,7 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
|
||||||
// Render the shapes
|
// Render the shapes
|
||||||
if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
|
if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
|
||||||
if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
|
if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
|
||||||
|
if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
|
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
|
||||||
|
|
|
@ -33,6 +33,8 @@
|
||||||
#include "SceneDemo.h"
|
#include "SceneDemo.h"
|
||||||
#include "Sphere.h"
|
#include "Sphere.h"
|
||||||
#include "Box.h"
|
#include "Box.h"
|
||||||
|
#include "Cone.h"
|
||||||
|
#include "Cylinder.h"
|
||||||
#include "Capsule.h"
|
#include "Capsule.h"
|
||||||
#include "Line.h"
|
#include "Line.h"
|
||||||
#include "ConvexMesh.h"
|
#include "ConvexMesh.h"
|
||||||
|
@ -136,6 +138,8 @@ class CollisionDetectionScene : public SceneDemo {
|
||||||
//Box* mBox;
|
//Box* mBox;
|
||||||
Sphere* mSphere1;
|
Sphere* mSphere1;
|
||||||
Sphere* mSphere2;
|
Sphere* mSphere2;
|
||||||
|
Capsule* mCapsule1;
|
||||||
|
Capsule* mCapsule2;
|
||||||
//Cone* mCone;
|
//Cone* mCone;
|
||||||
//Cylinder* mCylinder;
|
//Cylinder* mCylinder;
|
||||||
//Capsule* mCapsule;
|
//Capsule* mCapsule;
|
||||||
|
|
Loading…
Reference in New Issue
Block a user