// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) // SPDX-FileCopyrightText: 2021 Jorrit Rouwe // SPDX-License-Identifier: MIT #include #include #include #include #include #include #include #include #include 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 = 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(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(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(inShape1); JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated); const RotatedTranslatedShape *shape2 = static_cast(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(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(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(inShapeCast.mShape); JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated); const RotatedTranslatedShape *shape2 = static_cast(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