377 lines
17 KiB
C++
377 lines
17 KiB
C++
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include <Jolt/Jolt.h>
|
|
|
|
#include <Jolt/Physics/Vehicle/VehicleCollisionTester.h>
|
|
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
|
|
#include <Jolt/Physics/Collision/RayCast.h>
|
|
#include <Jolt/Physics/Collision/ShapeCast.h>
|
|
#include <Jolt/Physics/Collision/CastResult.h>
|
|
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
|
|
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
|
|
#include <Jolt/Physics/PhysicsSystem.h>
|
|
|
|
JPH_NAMESPACE_BEGIN
|
|
|
|
bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
|
|
{
|
|
const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
|
|
const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
|
|
|
|
const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
|
|
const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
|
|
|
|
const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
|
|
const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
|
|
|
|
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
|
|
float wheel_radius = wheel_settings->mRadius;
|
|
float ray_length = wheel_settings->mSuspensionMaxLength + wheel_radius;
|
|
RRayCast ray { inOrigin, ray_length * inDirection };
|
|
|
|
class MyCollector : public CastRayCollector
|
|
{
|
|
public:
|
|
MyCollector(PhysicsSystem &inPhysicsSystem, const RRayCast &inRay, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
|
|
mPhysicsSystem(inPhysicsSystem),
|
|
mRay(inRay),
|
|
mUpDirection(inUpDirection),
|
|
mCosMaxSlopeAngle(inCosMaxSlopeAngle)
|
|
{
|
|
}
|
|
|
|
virtual void AddHit(const RayCastResult &inResult) override
|
|
{
|
|
// Test if this collision is closer than the previous one
|
|
if (inResult.mFraction < GetEarlyOutFraction())
|
|
{
|
|
// Lock the body
|
|
BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID);
|
|
JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
|
|
const Body *body = &lock.GetBody();
|
|
|
|
if (body->IsSensor())
|
|
return;
|
|
|
|
// Test that we're not hitting a vertical wall
|
|
RVec3 contact_pos = mRay.GetPointOnRay(inResult.mFraction);
|
|
Vec3 normal = body->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, contact_pos);
|
|
if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
|
|
{
|
|
// Update early out fraction to this hit
|
|
UpdateEarlyOutFraction(inResult.mFraction);
|
|
|
|
// Get the contact properties
|
|
mBody = body;
|
|
mSubShapeID2 = inResult.mSubShapeID2;
|
|
mContactPosition = contact_pos;
|
|
mContactNormal = normal;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Configuration
|
|
PhysicsSystem & mPhysicsSystem;
|
|
RRayCast mRay;
|
|
Vec3 mUpDirection;
|
|
float mCosMaxSlopeAngle;
|
|
|
|
// Resulting closest collision
|
|
const Body * mBody = nullptr;
|
|
SubShapeID mSubShapeID2;
|
|
RVec3 mContactPosition;
|
|
Vec3 mContactNormal;
|
|
};
|
|
|
|
RayCastSettings settings;
|
|
|
|
MyCollector collector(inPhysicsSystem, ray, mUp, mCosMaxSlopeAngle);
|
|
inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphase_layer_filter, object_layer_filter, body_filter);
|
|
if (collector.mBody == nullptr)
|
|
return false;
|
|
|
|
outBody = const_cast<Body *>(collector.mBody);
|
|
outSubShapeID = collector.mSubShapeID2;
|
|
outContactPosition = collector.mContactPosition;
|
|
outContactNormal = collector.mContactNormal;
|
|
outSuspensionLength = max(0.0f, ray_length * collector.GetEarlyOutFraction() - wheel_radius);
|
|
|
|
return true;
|
|
}
|
|
|
|
void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
|
|
{
|
|
// Recalculate the contact points assuming the contact point is on an infinite plane
|
|
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
|
|
float d_dot_n = inDirection.Dot(ioContactNormal);
|
|
if (d_dot_n < -1.0e-6f)
|
|
{
|
|
// Reproject the contact position using the suspension ray and the plane formed by the contact position and normal
|
|
ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection;
|
|
|
|
// The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius
|
|
ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
|
|
}
|
|
else
|
|
{
|
|
// If the normal is pointing away we assume there's no collision anymore
|
|
ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
|
|
}
|
|
}
|
|
|
|
bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
|
|
{
|
|
const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
|
|
const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
|
|
|
|
const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
|
|
const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
|
|
|
|
const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
|
|
const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
|
|
|
|
SphereShape sphere(mRadius);
|
|
sphere.SetEmbedded();
|
|
|
|
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
|
|
float wheel_radius = wheel_settings->mRadius;
|
|
float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius;
|
|
RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length);
|
|
|
|
ShapeCastSettings settings;
|
|
settings.mUseShrunkenShapeAndConvexRadius = true;
|
|
settings.mReturnDeepestPoint = true;
|
|
|
|
class MyCollector : public CastShapeCollector
|
|
{
|
|
public:
|
|
MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
|
|
mPhysicsSystem(inPhysicsSystem),
|
|
mShapeCast(inShapeCast),
|
|
mUpDirection(inUpDirection),
|
|
mCosMaxSlopeAngle(inCosMaxSlopeAngle)
|
|
{
|
|
}
|
|
|
|
virtual void AddHit(const ShapeCastResult &inResult) override
|
|
{
|
|
// Test if this collision is closer/deeper than the previous one
|
|
float early_out = inResult.GetEarlyOutFraction();
|
|
if (early_out < GetEarlyOutFraction())
|
|
{
|
|
// Lock the body
|
|
BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
|
|
JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
|
|
const Body *body = &lock.GetBody();
|
|
|
|
if (body->IsSensor())
|
|
return;
|
|
|
|
// Test that we're not hitting a vertical wall
|
|
Vec3 normal = -inResult.mPenetrationAxis.Normalized();
|
|
if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
|
|
{
|
|
// Update early out fraction to this hit
|
|
UpdateEarlyOutFraction(early_out);
|
|
|
|
// Get the contact properties
|
|
mBody = body;
|
|
mSubShapeID2 = inResult.mSubShapeID2;
|
|
mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
|
|
mContactNormal = normal;
|
|
mFraction = inResult.mFraction;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Configuration
|
|
PhysicsSystem & mPhysicsSystem;
|
|
const RShapeCast & mShapeCast;
|
|
Vec3 mUpDirection;
|
|
float mCosMaxSlopeAngle;
|
|
|
|
// Resulting closest collision
|
|
const Body * mBody = nullptr;
|
|
SubShapeID mSubShapeID2;
|
|
RVec3 mContactPosition;
|
|
Vec3 mContactNormal;
|
|
float mFraction;
|
|
};
|
|
|
|
MyCollector collector(inPhysicsSystem, shape_cast, mUp, mCosMaxSlopeAngle);
|
|
inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
|
|
if (collector.mBody == nullptr)
|
|
return false;
|
|
|
|
outBody = const_cast<Body *>(collector.mBody);
|
|
outSubShapeID = collector.mSubShapeID2;
|
|
outContactPosition = collector.mContactPosition;
|
|
outContactNormal = collector.mContactNormal;
|
|
outSuspensionLength = max(0.0f, shape_cast_length * collector.mFraction + mRadius - wheel_radius);
|
|
|
|
return true;
|
|
}
|
|
|
|
void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
|
|
{
|
|
// Recalculate the contact points assuming the contact point is on an infinite plane
|
|
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
|
|
float d_dot_n = inDirection.Dot(ioContactNormal);
|
|
if (d_dot_n < -1.0e-6f)
|
|
{
|
|
// Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal
|
|
// This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction
|
|
float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal);
|
|
float fraction = (mRadius + oc_dot_n) / d_dot_n;
|
|
ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal;
|
|
|
|
// Calculate the new suspension length in the same way as the cast sphere normally does
|
|
ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
|
|
}
|
|
else
|
|
{
|
|
// If the normal is pointing away we assume there's no collision anymore
|
|
ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
|
|
}
|
|
}
|
|
|
|
bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
|
|
{
|
|
const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
|
|
const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
|
|
|
|
const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
|
|
const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
|
|
|
|
const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
|
|
const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
|
|
|
|
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
|
|
float max_suspension_length = wheel_settings->mSuspensionMaxLength;
|
|
|
|
// Get the wheel transform given that the cylinder rotates around the Y axis
|
|
RMat44 shape_cast_start = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
|
|
shape_cast_start.SetTranslation(inOrigin);
|
|
|
|
// Construct a cylinder with the dimensions of the wheel
|
|
float wheel_half_width = 0.5f * wheel_settings->mWidth;
|
|
CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction);
|
|
cylinder.SetEmbedded();
|
|
|
|
RShapeCast shape_cast(&cylinder, Vec3::sOne(), shape_cast_start, inDirection * max_suspension_length);
|
|
|
|
ShapeCastSettings settings;
|
|
settings.mUseShrunkenShapeAndConvexRadius = true;
|
|
settings.mReturnDeepestPoint = true;
|
|
|
|
class MyCollector : public CastShapeCollector
|
|
{
|
|
public:
|
|
MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast) :
|
|
mPhysicsSystem(inPhysicsSystem),
|
|
mShapeCast(inShapeCast)
|
|
{
|
|
}
|
|
|
|
virtual void AddHit(const ShapeCastResult &inResult) override
|
|
{
|
|
// Test if this collision is closer/deeper than the previous one
|
|
float early_out = inResult.GetEarlyOutFraction();
|
|
if (early_out < GetEarlyOutFraction())
|
|
{
|
|
// Lock the body
|
|
BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
|
|
JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
|
|
const Body *body = &lock.GetBody();
|
|
|
|
if (body->IsSensor())
|
|
return;
|
|
|
|
// Update early out fraction to this hit
|
|
UpdateEarlyOutFraction(early_out);
|
|
|
|
// Get the contact properties
|
|
mBody = body;
|
|
mSubShapeID2 = inResult.mSubShapeID2;
|
|
mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
|
|
mContactNormal = -inResult.mPenetrationAxis.Normalized();
|
|
mFraction = inResult.mFraction;
|
|
}
|
|
}
|
|
|
|
// Configuration
|
|
PhysicsSystem & mPhysicsSystem;
|
|
const RShapeCast & mShapeCast;
|
|
|
|
// Resulting closest collision
|
|
const Body * mBody = nullptr;
|
|
SubShapeID mSubShapeID2;
|
|
RVec3 mContactPosition;
|
|
Vec3 mContactNormal;
|
|
float mFraction;
|
|
};
|
|
|
|
MyCollector collector(inPhysicsSystem, shape_cast);
|
|
inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
|
|
if (collector.mBody == nullptr)
|
|
return false;
|
|
|
|
outBody = const_cast<Body *>(collector.mBody);
|
|
outSubShapeID = collector.mSubShapeID2;
|
|
outContactPosition = collector.mContactPosition;
|
|
outContactNormal = collector.mContactNormal;
|
|
outSuspensionLength = max_suspension_length * collector.mFraction;
|
|
|
|
return true;
|
|
}
|
|
|
|
void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
|
|
{
|
|
// Recalculate the contact points assuming the contact point is on an infinite plane
|
|
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
|
|
float d_dot_n = inDirection.Dot(ioContactNormal);
|
|
if (d_dot_n < -1.0e-6f)
|
|
{
|
|
// Wheel size
|
|
float half_width = 0.5f * wheel_settings->mWidth;
|
|
float radius = wheel_settings->mRadius;
|
|
|
|
// Get the inverse local space contact normal for a cylinder pointing along Y
|
|
RMat44 wheel_transform = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
|
|
Vec3 inverse_local_normal = -wheel_transform.Multiply3x3Transposed(ioContactNormal);
|
|
|
|
// Get the support point of this normal in local space of the cylinder
|
|
// See CylinderShape::Cylinder::GetSupport
|
|
float x = inverse_local_normal.GetX(), y = inverse_local_normal.GetY(), z = inverse_local_normal.GetZ();
|
|
float o = sqrt(Square(x) + Square(z));
|
|
Vec3 support_point;
|
|
if (o > 0.0f)
|
|
support_point = Vec3((radius * x) / o, Sign(y) * half_width, (radius * z) / o);
|
|
else
|
|
support_point = Vec3(0, Sign(y) * half_width, 0);
|
|
|
|
// Rotate back to world space
|
|
support_point = wheel_transform.Multiply3x3(support_point);
|
|
|
|
// Now we can use inOrigin + support_point as the start of a ray of our suspension to the contact plane
|
|
// as know that it is the first point on the wheel that will hit the plane
|
|
RVec3 origin = inOrigin + support_point;
|
|
|
|
// Calculate contact position and suspension length, the is the same as VehicleCollisionTesterRay
|
|
// but we don't need to take the radius into account anymore
|
|
Vec3 oc(ioContactPosition - origin);
|
|
ioContactPosition = origin + oc.Dot(ioContactNormal) / d_dot_n * inDirection;
|
|
ioSuspensionLength = Clamp(oc.Dot(inDirection), 0.0f, wheel_settings->mSuspensionMaxLength);
|
|
}
|
|
else
|
|
{
|
|
// If the normal is pointing away we assume there's no collision anymore
|
|
ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
|
|
}
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|