Added more 3D joints

supernovaengine · Oct 11, 2023 · 9bd2c7d · 9bd2c7d
1 parent c8b75f3
commit 9bd2c7d
Show file tree

Hide file tree

Showing 7 changed files with 220 additions and 11 deletions.
diff --git a/engine/core/component/Joint2DComponent.h b/engine/core/component/Joint2DComponent.h
@@ -19,7 +19,7 @@ namespace Supernova{
         WELD,
         FRICTION,
         MOTOR,
-        ROPE
+        ROPE // distance in Box2D
     };
 
     struct Joint2DComponent{

diff --git a/engine/core/component/Joint3DComponent.h b/engine/core/component/Joint3DComponent.h
@@ -13,7 +13,12 @@ namespace Supernova{
         FIXED,
         DISTANCE,
         POINT,
-        HINGE
+        HINGE,
+        CONE,
+        PRISMATIC, // slider in Jolt
+        SWINGTWIST,
+        SIXDOF,
+        PATH
     };
 
     struct Joint3DComponent{

diff --git a/engine/core/object/physics/Joint3D.cpp b/engine/core/object/physics/Joint3D.cpp
@@ -51,4 +51,30 @@ void Joint3D::setPointJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor){
 
 void Joint3D::setHingeJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor, Vector3 axis, Vector3 normal){
     scene->getSystem<PhysicsSystem>()->loadHingeJoint3D(getComponent<Joint3DComponent>(), bodyA, bodyB, worldAnchor, axis, normal);
+}
+
+void Joint3D::setConeJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor, Vector3 twistAxis){
+    scene->getSystem<PhysicsSystem>()->loadConeJoint3D(getComponent<Joint3DComponent>(), bodyA, bodyB, worldAnchor, twistAxis);
+}
+
+void Joint3D::setPrismaticJoint(Entity bodyA, Entity bodyB, Vector3 sliderAxis, float limitsMin, float limitsMax){
+    scene->getSystem<PhysicsSystem>()->loadPrismaticJoint3D(getComponent<Joint3DComponent>(), bodyA, bodyB, sliderAxis, limitsMin, limitsMax);
+}
+
+void Joint3D::setSwingTwistJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor, Vector3 twistAxis, Vector3 planeAxis, float normalHalfConeAngle, float planeHalfConeAngle, float twistMinAngle, float twistMaxAngle){
+    scene->getSystem<PhysicsSystem>()->loadSwingTwistJoint3D(getComponent<Joint3DComponent>(), bodyA, bodyB, worldAnchor, twistAxis, planeAxis, normalHalfConeAngle, planeHalfConeAngle, twistMinAngle, twistMaxAngle);
+}
+
+void Joint3D::setSixDOFJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchorOnBodyA, Vector3 worldAnchorOnBodyB, Vector3 axisX, Vector3 axisY){
+    scene->getSystem<PhysicsSystem>()->loadSixDOFJoint3D(getComponent<Joint3DComponent>(), bodyA, bodyB, worldAnchorOnBodyA, worldAnchorOnBodyB, axisX, axisY);
+}
+
+void Joint3D::setPathJoint(Entity bodyA, Entity bodyB, std::vector<Vector3> positions, std::vector<Vector3> tangents, std::vector<Vector3> normals, Vector3 pathPosition, bool isLooping){
+    scene->getSystem<PhysicsSystem>()->loadPathJoint3D(getComponent<Joint3DComponent>(), bodyA, bodyB, positions, tangents, normals, pathPosition, isLooping);
+}
+
+Joint3DType Joint3D::getType(){
+    Joint3DComponent& joint = getComponent<Joint3DComponent>();
+
+    return joint.type;
 }
diff --git a/engine/core/object/physics/Joint3D.h b/engine/core/object/physics/Joint3D.h
@@ -24,7 +24,12 @@ namespace Supernova{
         void setDistanceJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchorOnBodyA, Vector3 worldAnchorOnBodyB);    
         void setPointJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor);
         void setHingeJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor, Vector3 axis, Vector3 normal);
-
+        void setConeJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor, Vector3 twistAxis);
+        void setPrismaticJoint(Entity bodyA, Entity bodyB, Vector3 sliderAxis, float limitsMin, float limitsMax);
+        void setSwingTwistJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchor, Vector3 twistAxis, Vector3 planeAxis, float normalHalfConeAngle, float planeHalfConeAngle, float twistMinAngle, float twistMaxAngle);
+        void setSixDOFJoint(Entity bodyA, Entity bodyB, Vector3 worldAnchorOnBodyA, Vector3 worldAnchorOnBodyB, Vector3 axisX, Vector3 axisY);
+        void setPathJoint(Entity bodyA, Entity bodyB, std::vector<Vector3> positions, std::vector<Vector3> tangents, std::vector<Vector3> normals, Vector3 pathPosition, bool isLooping);
+
         Joint3DType getType();
     };
 }

diff --git a/engine/core/subsystem/PhysicsSystem.cpp b/engine/core/subsystem/PhysicsSystem.cpp
@@ -923,8 +923,7 @@ bool PhysicsSystem::loadPointJoint3D(Joint3DComponent& joint, Entity bodyA, Enti
         updateBody3DPosition(signatureB, bodyB, myBodyB, true);
 
         JPH::PointConstraintSettings settings;
-        settings.mPoint1 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
-        settings.mPoint2 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
+        settings.mPoint1 = settings.mPoint2 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
 
         joint.joint = settings.Create(*myBodyA.body, *myBodyB.body);
 
@@ -952,12 +951,9 @@ bool PhysicsSystem::loadHingeJoint3D(Joint3DComponent& joint, Entity bodyA, Enti
         updateBody3DPosition(signatureB, bodyB, myBodyB, true);
 
         JPH::HingeConstraintSettings settings;
-        settings.mPoint1 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
-        settings.mPoint2 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
-        settings.mHingeAxis1 = JPH::Vec3(axis.x, axis.y, axis.z);
-        settings.mHingeAxis2 = JPH::Vec3(axis.x, axis.y, axis.z);
-        settings.mNormalAxis1 = JPH::Vec3(normal.x, normal.y, normal.z);
-        settings.mNormalAxis2 = JPH::Vec3(normal.x, normal.y, normal.z);
+        settings.mPoint1 = settings.mPoint2 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
+        settings.mHingeAxis1 = settings.mHingeAxis2 = JPH::Vec3(axis.x, axis.y, axis.z);
+        settings.mNormalAxis1 = settings.mNormalAxis2 = JPH::Vec3(normal.x, normal.y, normal.z);
 
         joint.joint = settings.Create(*myBodyA.body, *myBodyB.body);
 
@@ -972,6 +968,172 @@ bool PhysicsSystem::loadHingeJoint3D(Joint3DComponent& joint, Entity bodyA, Enti
     return true;
 }
 
+bool PhysicsSystem::loadConeJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchor, Vector3 twistAxis){
+    Signature signatureA = scene->getSignature(bodyA);
+    Signature signatureB = scene->getSignature(bodyB);
+
+    if (signatureA.test(scene->getComponentType<Body3DComponent>()) && signatureB.test(scene->getComponentType<Body3DComponent>())){
+
+        Body3DComponent myBodyA = scene->getComponent<Body3DComponent>(bodyA);
+        Body3DComponent myBodyB = scene->getComponent<Body3DComponent>(bodyB);
+
+        updateBody3DPosition(signatureA, bodyA, myBodyA, true);
+        updateBody3DPosition(signatureB, bodyB, myBodyB, true);
+
+        JPH::ConeConstraintSettings settings;
+        settings.mPoint1 = settings.mPoint2 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
+        settings.mTwistAxis1 = settings.mTwistAxis2 = JPH::Vec3(twistAxis.x, twistAxis.y, twistAxis.z);
+
+        joint.joint = settings.Create(*myBodyA.body, *myBodyB.body);
+
+        world3D->AddConstraint(joint.joint);
+        joint.type = Joint3DType::CONE;
+
+    }else{
+        Log::error("Cannot create joint, error in bodyA or bodyB");
+        return false;
+    }
+
+    return true;
+}
+
+bool PhysicsSystem::loadPrismaticJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 sliderAxis, float limitsMin, float limitsMax){
+    Signature signatureA = scene->getSignature(bodyA);
+    Signature signatureB = scene->getSignature(bodyB);
+
+    if (signatureA.test(scene->getComponentType<Body3DComponent>()) && signatureB.test(scene->getComponentType<Body3DComponent>())){
+
+        Body3DComponent myBodyA = scene->getComponent<Body3DComponent>(bodyA);
+        Body3DComponent myBodyB = scene->getComponent<Body3DComponent>(bodyB);
+
+        updateBody3DPosition(signatureA, bodyA, myBodyA, true);
+        updateBody3DPosition(signatureB, bodyB, myBodyB, true);
+
+        JPH::SliderConstraintSettings settings;
+        settings.mAutoDetectPoint = true;
+        settings.SetSliderAxis(JPH::Vec3(sliderAxis.x, sliderAxis.y, sliderAxis.z));
+		settings.mLimitsMin = limitsMin;
+		settings.mLimitsMax = limitsMax;
+
+        joint.joint = settings.Create(*myBodyA.body, *myBodyB.body);
+
+        world3D->AddConstraint(joint.joint);
+        joint.type = Joint3DType::PRISMATIC;
+
+    }else{
+        Log::error("Cannot create joint, error in bodyA or bodyB");
+        return false;
+    }
+
+    return true;
+}
+
+bool PhysicsSystem::loadSwingTwistJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchor, Vector3 twistAxis, Vector3 planeAxis, float normalHalfConeAngle, float planeHalfConeAngle, float twistMinAngle, float twistMaxAngle){
+    Signature signatureA = scene->getSignature(bodyA);
+    Signature signatureB = scene->getSignature(bodyB);
+
+    if (signatureA.test(scene->getComponentType<Body3DComponent>()) && signatureB.test(scene->getComponentType<Body3DComponent>())){
+
+        Body3DComponent myBodyA = scene->getComponent<Body3DComponent>(bodyA);
+        Body3DComponent myBodyB = scene->getComponent<Body3DComponent>(bodyB);
+
+        updateBody3DPosition(signatureA, bodyA, myBodyA, true);
+        updateBody3DPosition(signatureB, bodyB, myBodyB, true);
+
+        JPH::SwingTwistConstraintSettings settings;
+        settings.mPosition1 = settings.mPosition2 = JPH::Vec3(anchor.x, anchor.y, anchor.z);
+        settings.mTwistAxis1 = settings.mTwistAxis2 = JPH::Vec3(twistAxis.x, twistAxis.y, twistAxis.z);
+        settings.mPlaneAxis1 = settings.mPlaneAxis2 = JPH::Vec3(planeAxis.x, planeAxis.y, planeAxis.z);
+        settings.mNormalHalfConeAngle = Angle::defaultToRad(normalHalfConeAngle);
+        settings.mPlaneHalfConeAngle = Angle::defaultToRad(planeHalfConeAngle);
+        settings.mTwistMinAngle = Angle::defaultToRad(twistMinAngle);
+        settings.mTwistMaxAngle = Angle::defaultToRad(twistMaxAngle);
+
+        joint.joint = settings.Create(*myBodyA.body, *myBodyB.body);
+
+        world3D->AddConstraint(joint.joint);
+        joint.type = Joint3DType::SWINGTWIST;
+
+    }else{
+        Log::error("Cannot create joint, error in bodyA or bodyB");
+        return false;
+    }
+
+    return true;
+}
+
+bool PhysicsSystem::loadSixDOFJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchorA, Vector3 anchorB, Vector3 axisX, Vector3 axisY){
+    Signature signatureA = scene->getSignature(bodyA);
+    Signature signatureB = scene->getSignature(bodyB);
+
+    if (signatureA.test(scene->getComponentType<Body3DComponent>()) && signatureB.test(scene->getComponentType<Body3DComponent>())){
+
+        Body3DComponent myBodyA = scene->getComponent<Body3DComponent>(bodyA);
+        Body3DComponent myBodyB = scene->getComponent<Body3DComponent>(bodyB);
+
+        updateBody3DPosition(signatureA, bodyA, myBodyA, true);
+        updateBody3DPosition(signatureB, bodyB, myBodyB, true);
+
+        JPH::SixDOFConstraintSettings settings;
+        settings.mPosition1 = JPH::Vec3(anchorA.x, anchorA.y, anchorA.z);
+        settings.mPosition2 = JPH::Vec3(anchorB.x, anchorB.y, anchorB.z);
+        settings.mAxisX1 = settings.mAxisX2 = JPH::Vec3(axisX.x, axisX.y, axisX.z);
+        settings.mAxisY1 = settings.mAxisY2 = JPH::Vec3(axisY.x, axisY.y, axisY.z);
+
+        joint.joint = settings.Create(*myBodyA.body, *myBodyB.body);
+
+        world3D->AddConstraint(joint.joint);
+        joint.type = Joint3DType::SIXDOF;
+
+    }else{
+        Log::error("Cannot create joint, error in bodyA or bodyB");
+        return false;
+    }
+
+    return true;
+}
+
+bool PhysicsSystem::loadPathJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, std::vector<Vector3> positions, std::vector<Vector3> tangents, std::vector<Vector3> normals, Vector3 pathPosition, bool isLooping){
+    Signature signatureA = scene->getSignature(bodyA);
+    Signature signatureB = scene->getSignature(bodyB);
+
+    if (signatureA.test(scene->getComponentType<Body3DComponent>()) && signatureB.test(scene->getComponentType<Body3DComponent>())){
+
+        Body3DComponent myBodyA = scene->getComponent<Body3DComponent>(bodyA);
+        Body3DComponent myBodyB = scene->getComponent<Body3DComponent>(bodyB);
+
+        updateBody3DPosition(signatureA, bodyA, myBodyA, true);
+        updateBody3DPosition(signatureB, bodyB, myBodyB, true);
+
+        JPH::Ref<JPH::PathConstraintPathHermite> path = new JPH::PathConstraintPathHermite;
+
+        if (positions.size() != normals.size() != tangents.size()){
+            Log::error("Cannot create joint, positions size is different from normals and tangents");
+            return false;
+        }
+
+        for (int i = 0; i < positions.size(); i++){
+            path->AddPoint(JPH::Vec3(positions[i].x, positions[i].y, positions[i].z), JPH::Vec3(tangents[i].x, tangents[i].y, tangents[i].z), JPH::Vec3(normals[i].x, normals[i].y, normals[i].z));
+        }
+        path->SetIsLooping(isLooping);
+
+        JPH::PathConstraintSettings settings;
+        settings.mPath = path;
+        settings.mPathPosition = JPH::Vec3(pathPosition.x, pathPosition.y, pathPosition.z);
+
+        joint.joint = settings.Create(*myBodyA.body, *myBodyB.body);
+
+        world3D->AddConstraint(joint.joint);
+        joint.type = Joint3DType::PATH;
+
+    }else{
+        Log::error("Cannot create joint, error in bodyA or bodyB");
+        return false;
+    }
+
+    return true;
+}
+
 void PhysicsSystem::load(){
 }
 

diff --git a/engine/core/subsystem/PhysicsSystem.h b/engine/core/subsystem/PhysicsSystem.h
@@ -118,6 +118,11 @@ namespace Supernova{
 		bool loadDistanceJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchorA, Vector3 anchorB);
 		bool loadPointJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchor);
 		bool loadHingeJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchor, Vector3 axis, Vector3 normal);
+		bool loadConeJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchor, Vector3 twistAxis);
+		bool loadPrismaticJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 sliderAxis, float limitsMin, float limitsMax);
+		bool loadSwingTwistJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchor, Vector3 twistAxis, Vector3 planeAxis, float normalHalfConeAngle, float planeHalfConeAngle, float twistMinAngle, float twistMaxAngle);
+		bool loadSixDOFJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, Vector3 anchorA, Vector3 anchorB, Vector3 axisX, Vector3 axisY);
+		bool loadPathJoint3D(Joint3DComponent& joint, Entity bodyA, Entity bodyB, std::vector<Vector3> positions, std::vector<Vector3> tangents, std::vector<Vector3> normals, Vector3 pathPosition, bool isLooping);
 
 		virtual void load();
 		virtual void destroy();

diff --git a/engine/core/util/JoltPhysicsAux.h b/engine/core/util/JoltPhysicsAux.h
@@ -22,6 +22,12 @@
 #include "Jolt/Physics/Constraints/DistanceConstraint.h"
 #include "Jolt/Physics/Constraints/PointConstraint.h"
 #include "Jolt/Physics/Constraints/HingeConstraint.h"
+#include "Jolt/Physics/Constraints/ConeConstraint.h"
+#include "Jolt/Physics/Constraints/SliderConstraint.h"
+#include "Jolt/Physics/Constraints/SwingTwistConstraint.h"
+#include "Jolt/Physics/Constraints/SixDOFConstraint.h"
+#include "Jolt/Physics/Constraints/PathConstraint.h"
+#include "Jolt/Physics/Constraints/PathConstraintPathHermite.h"
 
 
 // Based on: https://github.com/jrouwe/JoltPhysics/blob/master/HelloWorld/HelloWorld.cpp