334 lines
17 KiB
C++
334 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/Collision/Shape/RotatedTranslatedShape.h>
|
|
#include <Jolt/Physics/Collision/CollisionDispatch.h>
|
|
#include <Jolt/Physics/Collision/RayCast.h>
|
|
#include <Jolt/Physics/Collision/ShapeCast.h>
|
|
#include <Jolt/Physics/Collision/TransformedShape.h>
|
|
#include <Jolt/Core/StreamIn.h>
|
|
#include <Jolt/Core/StreamOut.h>
|
|
#include <Jolt/ObjectStream/TypeDeclarations.h>
|
|
|
|
JPH_NAMESPACE_BEGIN
|
|
|
|
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings)
|
|
{
|
|
JPH_ADD_BASE_CLASS(RotatedTranslatedShapeSettings, DecoratedShapeSettings)
|
|
|
|
JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mPosition)
|
|
JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mRotation)
|
|
}
|
|
|
|
ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const
|
|
{
|
|
if (mCachedResult.IsEmpty())
|
|
Ref<Shape> shape = new RotatedTranslatedShape(*this, mCachedResult);
|
|
return mCachedResult;
|
|
}
|
|
|
|
RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult) :
|
|
DecoratedShape(EShapeSubType::RotatedTranslated, inSettings, outResult)
|
|
{
|
|
if (outResult.HasError())
|
|
return;
|
|
|
|
// Calculate center of mass position
|
|
mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass();
|
|
|
|
// Store rotation (position is always zero because we center around the center of mass)
|
|
mRotation = inSettings.mRotation;
|
|
mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
|
|
|
|
outResult.Set(this);
|
|
}
|
|
|
|
RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) :
|
|
DecoratedShape(EShapeSubType::RotatedTranslated, inShape)
|
|
{
|
|
// Calculate center of mass position
|
|
mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass();
|
|
|
|
// Store rotation (position is always zero because we center around the center of mass)
|
|
mRotation = inRotation;
|
|
mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
|
|
}
|
|
|
|
MassProperties RotatedTranslatedShape::GetMassProperties() const
|
|
{
|
|
// Rotate inertia of child into place
|
|
MassProperties p = mInnerShape->GetMassProperties();
|
|
p.Rotate(Mat44::sRotation(mRotation));
|
|
return p;
|
|
}
|
|
|
|
AABox RotatedTranslatedShape::GetLocalBounds() const
|
|
{
|
|
return mInnerShape->GetLocalBounds().Transformed(Mat44::sRotation(mRotation));
|
|
}
|
|
|
|
AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
|
|
{
|
|
Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
|
|
return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale));
|
|
}
|
|
|
|
TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
|
|
{
|
|
// We don't use any bits in the sub shape ID
|
|
outRemainder = inSubShapeID;
|
|
|
|
TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID());
|
|
ts.SetShapeScale(TransformScale(inScale));
|
|
return ts;
|
|
}
|
|
|
|
Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
|
|
{
|
|
// Transform surface position to local space and pass call on
|
|
Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
|
|
Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition);
|
|
|
|
// Transform normal to this shape's space
|
|
return transform.Multiply3x3Transposed(normal);
|
|
}
|
|
|
|
void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
|
|
{
|
|
Mat44 transform = Mat44::sRotation(mRotation);
|
|
mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
|
|
}
|
|
|
|
void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
|
|
{
|
|
// Get center of mass transform of child
|
|
Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
|
|
|
|
// Recurse to child
|
|
mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
|
|
{
|
|
mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
|
|
}
|
|
|
|
void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
|
|
{
|
|
mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection);
|
|
}
|
|
|
|
void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
|
|
{
|
|
mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale));
|
|
}
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
|
|
{
|
|
// Transform the ray
|
|
Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
|
|
RayCast ray = inRay.Transformed(transform);
|
|
|
|
return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit);
|
|
}
|
|
|
|
void RotatedTranslatedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
|
|
{
|
|
// Test shape filter
|
|
if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
|
|
return;
|
|
|
|
// Transform the ray
|
|
Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
|
|
RayCast ray = inRay.Transformed(transform);
|
|
|
|
return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
|
|
}
|
|
|
|
void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
|
|
{
|
|
// Test shape filter
|
|
if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
|
|
return;
|
|
|
|
// Transform the point
|
|
Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
|
|
mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
|
|
}
|
|
|
|
void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
|
|
{
|
|
mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
|
|
}
|
|
|
|
void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
|
|
{
|
|
// Test shape filter
|
|
if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
|
|
return;
|
|
|
|
mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector, inShapeFilter);
|
|
}
|
|
|
|
void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
|
|
{
|
|
mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioCollector);
|
|
}
|
|
|
|
void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
|
|
{
|
|
JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
|
|
|
|
// Get world transform of 1
|
|
Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
|
|
|
|
CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, shape1->TransformScale(inScale1), inScale2, transform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
|
|
}
|
|
|
|
void RotatedTranslatedShape::sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
|
|
{
|
|
JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
|
|
|
|
// Get world transform of 2
|
|
Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
|
|
|
|
CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, shape2->TransformScale(inScale2), inCenterOfMassTransform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
|
|
}
|
|
|
|
void RotatedTranslatedShape::sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
|
|
{
|
|
JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
|
|
JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
|
|
|
|
// Get world transform of 1 and 2
|
|
Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
|
|
Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
|
|
|
|
CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, shape2->mInnerShape, shape1->TransformScale(inScale1), shape2->TransformScale(inScale2), transform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
|
|
}
|
|
|
|
void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
|
|
{
|
|
// Fetch rotated translated shape from cast shape
|
|
JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
|
|
|
|
// Transform the shape cast and update the shape
|
|
Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation);
|
|
Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
|
|
ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection);
|
|
|
|
CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
|
|
}
|
|
|
|
void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
|
|
{
|
|
JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape = static_cast<const RotatedTranslatedShape *>(inShape);
|
|
|
|
// Determine the local transform
|
|
Mat44 local_transform = Mat44::sRotation(shape->mRotation);
|
|
|
|
// Transform the shape cast
|
|
ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3());
|
|
|
|
CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
|
|
}
|
|
|
|
void RotatedTranslatedShape::sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
|
|
{
|
|
JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
|
|
JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
|
|
const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape);
|
|
|
|
// Determine the local transform of shape 2
|
|
Mat44 local_transform2 = Mat44::sRotation(shape2->mRotation);
|
|
Mat44 local_transform2_transposed = local_transform2.Transposed3x3();
|
|
|
|
// Transform the shape cast and update the shape
|
|
Mat44 transform = (local_transform2_transposed * inShapeCast.mCenterOfMassStart) * Mat44::sRotation(shape1->mRotation);
|
|
Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
|
|
ShapeCast shape_cast(shape1->mInnerShape, scale, transform, local_transform2_transposed.Multiply3x3(inShapeCast.mDirection));
|
|
|
|
CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape2->mInnerShape, shape2->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
|
|
}
|
|
|
|
void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const
|
|
{
|
|
DecoratedShape::SaveBinaryState(inStream);
|
|
|
|
inStream.Write(mCenterOfMass);
|
|
inStream.Write(mRotation);
|
|
}
|
|
|
|
void RotatedTranslatedShape::RestoreBinaryState(StreamIn &inStream)
|
|
{
|
|
DecoratedShape::RestoreBinaryState(inStream);
|
|
|
|
inStream.Read(mCenterOfMass);
|
|
inStream.Read(mRotation);
|
|
mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
|
|
}
|
|
|
|
bool RotatedTranslatedShape::IsValidScale(Vec3Arg inScale) const
|
|
{
|
|
if (!Shape::IsValidScale(inScale))
|
|
return false;
|
|
|
|
if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
|
|
return mInnerShape->IsValidScale(inScale);
|
|
|
|
if (!ScaleHelpers::CanScaleBeRotated(mRotation, inScale))
|
|
return false;
|
|
|
|
return mInnerShape->IsValidScale(ScaleHelpers::RotateScale(mRotation, inScale));
|
|
}
|
|
|
|
Vec3 RotatedTranslatedShape::MakeScaleValid(Vec3Arg inScale) const
|
|
{
|
|
Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
|
|
|
|
if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(scale))
|
|
return mInnerShape->MakeScaleValid(scale);
|
|
|
|
if (ScaleHelpers::CanScaleBeRotated(mRotation, scale))
|
|
return ScaleHelpers::RotateScale(mRotation.Conjugated(), mInnerShape->MakeScaleValid(ScaleHelpers::RotateScale(mRotation, scale)));
|
|
|
|
Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs());
|
|
Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale;
|
|
if (ScaleHelpers::CanScaleBeRotated(mRotation, uniform_scale))
|
|
return uniform_scale;
|
|
|
|
return Sign(scale.GetX()) * abs_uniform_scale;
|
|
}
|
|
|
|
void RotatedTranslatedShape::sRegister()
|
|
{
|
|
ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::RotatedTranslated);
|
|
f.mConstruct = []() -> Shape * { return new RotatedTranslatedShape; };
|
|
f.mColor = Color::sBlue;
|
|
|
|
for (EShapeSubType s : sAllSubShapeTypes)
|
|
{
|
|
CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape);
|
|
CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated);
|
|
CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape);
|
|
CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated);
|
|
}
|
|
|
|
CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCollideRotatedTranslatedVsRotatedTranslated);
|
|
CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsRotatedTranslated);
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|