diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index fa94024e..636fcab2 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -291,6 +291,8 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const { + PROFILE("CollisionDetection::raycast()"); + RaycastTest rayCastTest(raycastCallback); // Ask the broad-phase algorithm to call the testRaycastAgainstShape() diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index f484d636..fb2d6497 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -62,3 +62,31 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) { return mCollisionShape->testPointInside(localPoint, this); } +// Raycast method with feedback information +/** + * @param ray Ray to use for the raycasting + * @param[out] raycastInfo Result of the raycasting that is valid only if the + * methods returned true + * @return True if the ray hit the collision shape + */ +bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { + + // If the corresponding body is not active, it cannot be hit by rays + if (!mBody->isActive()) return false; + + // Convert the ray into the local-space of the collision shape + const Transform localToWorldTransform = getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + Ray rayLocal(worldToLocalTransform * ray.point1, + worldToLocalTransform * ray.point2, + ray.maxFraction); + + bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); + + // Convert the raycast info into world-space + raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal; + raycastInfo.worldNormal.normalize(); + + return isHit; +} diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index fae65c45..3df5dcac 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -251,21 +251,6 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const { return mBody->mTransform * mLocalToBodyTransform; } -// Raycast method with feedback information -/** - * @param ray Ray to use for the raycasting - * @param[out] raycastInfo Result of the raycasting that is valid only if the - * methods returned true - * @return True if the ray hit the collision shape - */ -inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { - - // If the corresponding body is not active, it cannot be hit by rays - if (!mBody->isActive()) return false; - - return mCollisionShape->raycast(ray, raycastInfo, this); -} - // Return the next proxy shape in the linked list of proxy shapes /** * @return Pointer to the next proxy shape in the linked list of proxy shapes diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index bf893ab8..d009b52c 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -31,6 +31,7 @@ #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "DynamicAABBTree.h" +#include "engine/Profiler.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -234,6 +235,8 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { + PROFILE("BroadPhaseAlgorithm::raycast()"); + BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index a1b19113..92e4656e 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -633,11 +633,12 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, // Ray casting method void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const { + PROFILE("DynamicAABBTree::raycast()"); + decimal maxFraction = ray.maxFraction; // Create an AABB for the ray - Vector3 endPoint = ray.point1 + - maxFraction * (ray.point2 - ray.point1); + Vector3 endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1); AABB rayAABB(Vector3::min(ray.point1, endPoint), Vector3::max(ray.point1, endPoint)); diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 35587c93..ddfedf36 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -433,11 +433,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& const decimal epsilon = decimal(0.0001); // Convert the ray origin and direction into the local-space of the collision shape - const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - Vector3 point1 = worldToLocalTransform * ray.point1; - Vector3 point2 = worldToLocalTransform * ray.point2; - Vector3 rayDirection = point2 - point1; + Vector3 rayDirection = ray.point2 - ray.point1; // If the points of the segment are two close, return no hit if (rayDirection.lengthSquare() < machineEpsilonSquare) return false; @@ -449,7 +445,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); decimal lambda = decimal(0.0); - suppA = point1; // Current lower bound point on the ray (starting at ray's origin) + suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); Vector3 v = suppA - suppB; decimal vDotW, vDotR; @@ -476,7 +472,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& // We have found a better lower bound for the hit point along the ray lambda = lambda - vDotW / vDotR; - suppA = point1 + lambda * rayDirection; + suppA = ray.point1 + lambda * rayDirection; w = suppA - suppB; n = v; } @@ -512,12 +508,12 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& // A raycast hit has been found, we fill in the raycast info raycastInfo.hitFraction = lambda; - raycastInfo.worldPoint = localToWorldTransform * pointB; + raycastInfo.worldPoint = pointB; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * n.getUnit(); + raycastInfo.worldNormal = n; } else { // Degenerated normal vector, we return a zero normal vector raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0)); diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 33dbb490..4d8d488f 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -69,11 +69,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const // Raycast method with feedback information bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - const Vector3 point1 = worldToLocalTransform * ray.point1; - const Vector3 point2 = worldToLocalTransform * ray.point2; - Vector3 rayDirection = point2 - point1; + Vector3 rayDirection = ray.point2 - ray.point1; decimal tMin = DECIMAL_SMALLEST; decimal tMax = DECIMAL_LARGEST; Vector3 normalDirection(decimal(0), decimal(0), decimal(0)); @@ -86,14 +82,14 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro if (std::abs(rayDirection[i]) < MACHINE_EPSILON) { // If the ray's origin is not inside the slab, there is no hit - if (point1[i] > mExtent[i] || point1[i] < -mExtent[i]) return false; + if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false; } else { // Compute the intersection of the ray with the near and far plane of the slab decimal oneOverD = decimal(1.0) / rayDirection[i]; - decimal t1 = (-mExtent[i] - point1[i]) * oneOverD; - decimal t2 = (mExtent[i] - point1[i]) * oneOverD; + decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD; + decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD; currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0); currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0); currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0); @@ -124,14 +120,13 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro if (tMin < decimal(0.0)) return false; // The ray intersects the three slabs, we compute the hit point - Vector3 localHitPoint = point1 + tMin * rayDirection; + Vector3 localHitPoint = ray.point1 + tMin * rayDirection; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = tMin; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; - normalDirection.normalize(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldPoint = localHitPoint; + raycastInfo.worldNormal = normalDirection; return true; } diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index 22d44c69..b5efbc5a 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -149,18 +149,13 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS // Raycast method with feedback information bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - // Transform the ray direction and origin in local-space coordinates - const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - const Vector3 point1 = worldToLocalTransform * ray.point1; - const Vector3 point2 = worldToLocalTransform * ray.point2; - const Vector3 n = point2 - point1; + const Vector3 n = ray.point2 - ray.point1; const decimal epsilon = decimal(0.01); Vector3 p(decimal(0), -mHalfHeight, decimal(0)); Vector3 q(decimal(0), mHalfHeight, decimal(0)); Vector3 d = q - p; - Vector3 m = point1 - p; + Vector3 m = ray.point1 - p; decimal t; decimal mDotD = m.dot(d); @@ -168,7 +163,7 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* decimal dDotD = d.dot(d); // Test if the segment is outside the cylinder - decimal vec1DotD = (point1 - Vector3(decimal(0.0), -mHalfHeight - mRadius, decimal(0.0))).dot(d); + decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mRadius, decimal(0.0))).dot(d); if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false; decimal ddotDExtraCaps = decimal(2.0) * mRadius * d.y; if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; @@ -194,13 +189,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* // Check intersection between the ray and the "p" sphere endcap of the capsule Vector3 hitLocalPoint; decimal hitFraction; - if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { + if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; - Vector3 normalDirection = (hitLocalPoint - p).getUnit(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - p; + raycastInfo.worldNormal = normalDirection; return true; } @@ -212,13 +207,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* // Check intersection between the ray and the "q" sphere endcap of the capsule Vector3 hitLocalPoint; decimal hitFraction; - if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { + if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; - Vector3 normalDirection = (hitLocalPoint - q).getUnit(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - q; + raycastInfo.worldNormal = normalDirection; return true; } @@ -245,13 +240,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* // Check intersection between the ray and the "p" sphere endcap of the capsule Vector3 hitLocalPoint; decimal hitFraction; - if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { + if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; - Vector3 normalDirection = (hitLocalPoint - p).getUnit(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - p; + raycastInfo.worldNormal = normalDirection; return true; } @@ -263,13 +258,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* // Check intersection between the ray and the "q" sphere endcap of the capsule Vector3 hitLocalPoint; decimal hitFraction; - if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { + if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint; - Vector3 normalDirection = (hitLocalPoint - q).getUnit(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - q; + raycastInfo.worldNormal = normalDirection; return true; } @@ -284,15 +279,15 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* if (t < decimal(0.0) || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = point1 + t * n; + Vector3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + raycastInfo.worldPoint = localHitPoint; Vector3 v = localHitPoint - p; Vector3 w = (v.dot(d) / d.lengthSquare()) * d; Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldNormal = normalDirection; return true; } diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 84a4da43..6ae54d90 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -162,6 +162,8 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& /// the ray hits many triangles. bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { + PROFILE("ConcaveMeshShape::raycast()"); + // Create the callback object that will compute ray casting against triangles ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray); @@ -170,6 +172,8 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh // in the hit AABBs. mDynamicAABBTree.raycast(ray, raycastCallback); + raycastCallback.raycastTriangles(); + return raycastCallback.getIsHit(); } diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 6c0e8a04..76bbddbf 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -31,6 +31,7 @@ #include "collision/broadphase/DynamicAABBTree.h" #include "collision/TriangleMesh.h" #include "collision/shapes/TriangleShape.h" +#include "engine/Profiler.h" namespace reactphysics3d { diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index 1a4ccda7..bd96b6d0 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -99,12 +99,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, // http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - // Transform the ray direction and origin in local-space coordinates - const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - const Vector3 point1 = worldToLocalTransform * ray.point1; - const Vector3 point2 = worldToLocalTransform * ray.point2; - const Vector3 r = point2 - point1; + const Vector3 r = ray.point2 - ray.point1; const decimal epsilon = decimal(0.00001); Vector3 V(0, mHalfHeight, 0); @@ -113,7 +108,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight; decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); decimal factor = decimal(1.0) - cosThetaSquare; - Vector3 delta = point1 - V; + Vector3 delta = ray.point1 - V; decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - cosThetaSquare * delta.z * delta.z; decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; @@ -159,10 +154,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr } // If the origin of the ray is inside the cone, we return no hit - if (testPointInside(point1, NULL)) return false; + if (testPointInside(ray.point1, NULL)) return false; - localHitPoint[0] = point1 + tHit[0] * r; - localHitPoint[1] = point1 + tHit[1] * r; + localHitPoint[0] = ray.point1 + tHit[0] * r; + localHitPoint[1] = ray.point1 + tHit[1] * r; // Only keep hit points in one side of the double cone (the cone we are interested in) if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) { @@ -184,10 +179,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr if (r.y > epsilon) { // Compute the intersection with the base plane of the cone - tHit[2] = (-point1.y - mHalfHeight) / (r.y); + tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); // Only keep this intersection if it is inside the cone radius - localHitPoint[2] = point1 + tHit[2] * r; + localHitPoint[2] = ray.point1 + tHit[2] * r; if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { tHit[2] = decimal(-1.0); @@ -229,8 +224,8 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint[hitIndex]; - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localNormal[hitIndex]; + raycastInfo.worldPoint = localHitPoint[hitIndex]; + raycastInfo.worldNormal = localNormal[hitIndex]; return true; } diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 1388ab51..91fd8f40 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -92,18 +92,13 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio /// Morgan Kaufmann. bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - // Transform the ray direction and origin in local-space coordinates - const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - const Vector3 pointA = worldToLocalTransform * ray.point1; - const Vector3 pointB = worldToLocalTransform * ray.point2; - const Vector3 n = pointB - pointA; + const Vector3 n = ray.point2 - ray.point1; const decimal epsilon = decimal(0.01); Vector3 p(decimal(0), -mHalfHeight, decimal(0)); Vector3 q(decimal(0), mHalfHeight, decimal(0)); Vector3 d = q - p; - Vector3 m = pointA - p; + Vector3 m = ray.point1 - p; decimal t; decimal mDotD = m.dot(d); @@ -139,13 +134,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape if (t < decimal(0.0) || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = pointA + t * n; + Vector3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + raycastInfo.worldPoint = localHitPoint; Vector3 normalDirection(0, decimal(-1), 0); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldNormal = normalDirection; return true; } @@ -158,13 +153,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape if (t < decimal(0.0) || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = pointA + t * n; + Vector3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + raycastInfo.worldPoint = localHitPoint; Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldNormal = normalDirection; return true; } @@ -199,13 +194,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape if (t < decimal(0.0) || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = pointA + t * n; + Vector3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + raycastInfo.worldPoint = localHitPoint; Vector3 normalDirection(0, decimal(-1.0), 0); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldNormal = normalDirection; return true; } @@ -226,13 +221,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape if (t < decimal(0.0) || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = pointA + t * n; + Vector3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + raycastInfo.worldPoint = localHitPoint; Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + raycastInfo.worldNormal = normalDirection; return true; } @@ -244,15 +239,15 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape if (t < decimal(0.0) || t > ray.maxFraction) return false; // Compute the hit information - Vector3 localHitPoint = pointA + t * n; + Vector3 localHitPoint = ray.point1 + t * n; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; + raycastInfo.worldPoint = localHitPoint; Vector3 v = localHitPoint - p; Vector3 w = (v.dot(d) / d.lengthSquare()) * d; - Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection; + Vector3 normalDirection = (localHitPoint - (p + w)); + raycastInfo.worldNormal = normalDirection; return true; } diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 9b231d0a..779b8f15 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -47,10 +47,7 @@ SphereShape::~SphereShape() { // Raycast method with feedback information bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - // We perform the intersection test in world-space - - const Vector3 sphereCenter = proxyShape->getLocalToWorldTransform().getPosition(); - const Vector3 m = ray.point1 - sphereCenter; + const Vector3 m = ray.point1; decimal c = m.dot(m) - mMargin * mMargin; // If the origin of the ray is inside the sphere, we return no intersection @@ -85,7 +82,7 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = ray.point1 + t * rayDirection; - raycastInfo.worldNormal = (raycastInfo.worldPoint - sphereCenter).getUnit(); + raycastInfo.worldNormal = raycastInfo.worldPoint; return true; } diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index bef7f8e0..1731193b 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -26,6 +26,7 @@ // Libraries #include "TriangleShape.h" #include "collision/ProxyShape.h" +#include "engine/Profiler.h" #include "configuration.h" #include @@ -56,35 +57,45 @@ TriangleShape::~TriangleShape() { /// Real-time Collision Detection by Christer Ericson. bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - // TODO : For all collision shape, try to perform raycasting in local-space and - // compute world-space hit point, normal in upper classes when local-space - // hit points are returned - const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - const Vector3 point1 = worldToLocalTransform * ray.point1; - const Vector3 point2 = worldToLocalTransform * ray.point2; + PROFILE("TriangleShape::raycast()"); - const Vector3 pq = point2 - point1; - const Vector3 pa = mPoints[0] - point1; - const Vector3 pb = mPoints[1] - point1; - const Vector3 pc = mPoints[2] - point1; + const Vector3 pq = ray.point2 - ray.point1; + const Vector3 pa = mPoints[0] - ray.point1; + const Vector3 pb = mPoints[1] - ray.point1; + const Vector3 pc = mPoints[2] - ray.point1; // Test if the line PQ is inside the eges BC, CA and AB. We use the triple // product for this test. const Vector3 m = pq.cross(pc); decimal u = pb.dot(m); - if (mRaycastTestType == FRONT && u < decimal(0.0)) return false; - if (mRaycastTestType == BACK && u > decimal(0.0)) return false; + if (mRaycastTestType == FRONT) { + if (u < decimal(0.0)) return false; + } + else if (mRaycastTestType == BACK) { + if (u > decimal(0.0)) return false; + } decimal v = -pa.dot(m); - if (mRaycastTestType == FRONT && v < decimal(0.0)) return false; - if (mRaycastTestType == BACK && v > decimal(0.0)) return false; - if (mRaycastTestType == FRONT_AND_BACK && !sameSign(u, v)) return false; + if (mRaycastTestType == FRONT) { + if (v < decimal(0.0)) return false; + } + else if (mRaycastTestType == BACK) { + if (v > decimal(0.0)) return false; + } + else if (mRaycastTestType == FRONT_AND_BACK) { + if (!sameSign(u, v)) return false; + } decimal w = pa.dot(pq.cross(pb)); - if (mRaycastTestType == FRONT && w < decimal(0.0)) return false; - if (mRaycastTestType == BACK && w > decimal(0.0)) return false; - if (mRaycastTestType == FRONT_AND_BACK && !sameSign(u, w)) return false; + if (mRaycastTestType == FRONT) { + if (w < decimal(0.0)) return false; + } + else if (mRaycastTestType == BACK) { + if (w > decimal(0.0)) return false; + } + else if (mRaycastTestType == FRONT_AND_BACK) { + if (!sameSign(u, w)) return false; + } // If the line PQ is in the triangle plane (case where u=v=w=0) if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false; @@ -104,10 +115,9 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; - raycastInfo.worldPoint = localToWorldTransform * localHitPoint; - raycastInfo.hitFraction = (localHitPoint - point1).length() / pq.length(); - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localHitNormal; - raycastInfo.worldNormal.normalize(); + raycastInfo.worldPoint = localHitPoint; + raycastInfo.hitFraction = (localHitPoint - ray.point1).length() / pq.length(); + raycastInfo.worldNormal = localHitNormal; return true; }