From 9fb1e3979820135fcee98f1a328d75118e59ed83 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 24 Jun 2020 22:21:54 +0200 Subject: [PATCH] Add constructors with local-space anchor/axis to BallAndSocketJointInfo, HingeJointInfo, SliderJointInfo, FixedJointInfo --- CHANGELOG.md | 3 + .../constraint/BallAndSocketJoint.h | 25 ++++ .../reactphysics3d/constraint/FixedJoint.h | 25 ++++ .../reactphysics3d/constraint/HingeJoint.h | 107 +++++++++++++++++- .../reactphysics3d/constraint/SliderJoint.h | 94 ++++++++++++++- include/reactphysics3d/engine/PhysicsWorld.h | 1 - src/constraint/BallAndSocketJoint.cpp | 24 +++- src/constraint/FixedJoint.cpp | 23 +++- src/constraint/HingeJoint.cpp | 41 +++++-- src/constraint/SliderJoint.cpp | 44 +++++-- 10 files changed, 349 insertions(+), 38 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2c322eb0..0aae2cd3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ do not hesitate to take a look at the user manual. - Method RigidBody::resetForce() to reset the accumulated external force on a rigid body has beend added - Method RigidBody::resetTorque() to reset the accumulated external torque on a rigid body has beend added + - Constructors with local-space anchor/axis have been added to BallAndSocketJointInfo, HingeJointInfo, FixedJointInfo and SliderJointInfo classes ### Changed @@ -16,6 +17,8 @@ do not hesitate to take a look at the user manual. ### Fixed +- Issue with concave vs convex shape collision detection has been fixed + ## Version 0.8.0 (May 31, 2020) Note that this release contains some public API changes. Please read carefully the following changes before upgrading to this new version and diff --git a/include/reactphysics3d/constraint/BallAndSocketJoint.h b/include/reactphysics3d/constraint/BallAndSocketJoint.h index 2e672344..fe834a6b 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/include/reactphysics3d/constraint/BallAndSocketJoint.h @@ -43,9 +43,18 @@ struct BallAndSocketJointInfo : public JointInfo { // -------------------- Attributes -------------------- // + /// True if this object has been constructed using local-space anchors + bool isUsingLocalSpaceAnchors; + /// Anchor point (in world-space coordinates) Vector3 anchorPointWorldSpace; + /// Anchor point on body 1 (in local-space coordinates) + Vector3 anchorPointBody1LocalSpace; + + /// Anchor point on body 2 (in local-space coordinates) + Vector3 anchorPointBody2LocalSpace; + /// Constructor /** * @param rigidBody1 Pointer to the first body of the joint @@ -56,7 +65,23 @@ struct BallAndSocketJointInfo : public JointInfo { BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace) : JointInfo(rigidBody1, rigidBody2, JointType::BALLSOCKETJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace) {} + + /// Constructor + /** + * @param rigidBody1 Pointer to the first body of the joint + * @param rigidBody2 Pointer to the second body of the joint + * @param anchorPointBody1LocalSpace The anchor point on body 1 in local-space coordinates + * @param anchorPointBody2LocalSpace The anchor point on body 2 in local-space coordinates + */ + BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1LocalSpace, + const Vector3& anchorPointBody2LocalSpace) + : JointInfo(rigidBody1, rigidBody2, JointType::BALLSOCKETJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1LocalSpace), + anchorPointBody2LocalSpace(anchorPointBody2LocalSpace) {} }; // Class BallAndSocketJoint diff --git a/include/reactphysics3d/constraint/FixedJoint.h b/include/reactphysics3d/constraint/FixedJoint.h index 2e491e81..84b7508a 100644 --- a/include/reactphysics3d/constraint/FixedJoint.h +++ b/include/reactphysics3d/constraint/FixedJoint.h @@ -43,9 +43,18 @@ struct FixedJointInfo : public JointInfo { // -------------------- Attributes -------------------- // + /// True if this object has been constructed using local-space anchors + bool isUsingLocalSpaceAnchors; + /// Anchor point (in world-space coordinates) Vector3 anchorPointWorldSpace; + /// Anchor point on body 1 (in local-space coordinates) + Vector3 anchorPointBody1LocalSpace; + + /// Anchor point on body 2 (in local-space coordinates) + Vector3 anchorPointBody2LocalSpace; + /// Constructor /** * @param rigidBody1 The first body of the joint @@ -56,7 +65,23 @@ struct FixedJointInfo : public JointInfo { FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace) : JointInfo(rigidBody1, rigidBody2, JointType::FIXEDJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace){} + + /// Constructor + /** + * @param rigidBody1 Pointer to the first body of the joint + * @param rigidBody2 Pointer to the second body of the joint + * @param anchorPointBody1LocalSpace The anchor point on body 1 in local-space coordinates + * @param anchorPointBody2LocalSpace The anchor point on body 2 in local-space coordinates + */ + FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1LocalSpace, + const Vector3& anchorPointBody2LocalSpace) + : JointInfo(rigidBody1, rigidBody2, JointType::FIXEDJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1LocalSpace), + anchorPointBody2LocalSpace(anchorPointBody2LocalSpace) {} }; // Class FixedJoint diff --git a/include/reactphysics3d/constraint/HingeJoint.h b/include/reactphysics3d/constraint/HingeJoint.h index 10b0c590..ed583e02 100644 --- a/include/reactphysics3d/constraint/HingeJoint.h +++ b/include/reactphysics3d/constraint/HingeJoint.h @@ -43,12 +43,27 @@ struct HingeJointInfo : public JointInfo { // -------------------- Attributes -------------------- // + /// True if this object has been constructed using local-space anchors + bool isUsingLocalSpaceAnchors; + /// Anchor point (in world-space coordinates) Vector3 anchorPointWorldSpace; + /// Anchor point on body 1 (in local-space coordinates) + Vector3 anchorPointBody1LocalSpace; + + /// Anchor point on body 2 (in local-space coordinates) + Vector3 anchorPointBody2LocalSpace; + /// Hinge rotation axis (in world-space coordinates) Vector3 rotationAxisWorld; + /// Hinge rotation axis of body 1 (in local-space coordinates) + Vector3 rotationAxisBody1Local; + + /// Hinge rotation axis of body 2 (in local-space coordinates) + Vector3 rotationAxisBody2Local; + /// True if the hinge joint limits are enabled bool isLimitEnabled; @@ -70,7 +85,7 @@ struct HingeJointInfo : public JointInfo { /// to desired motor speed decimal maxMotorTorque; - /// Constructor without limits and without motor + /// Constructor without limits and without motor with world-space anchor /** * @param rigidBody1 The first body of the joint * @param rigidBody2 The second body of the joint @@ -83,12 +98,13 @@ struct HingeJointInfo : public JointInfo { const Vector3& initAnchorPointWorldSpace, const Vector3& initRotationAxisWorld) : JointInfo(rigidBody1, rigidBody2, JointType::HINGEJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), motorSpeed(0), maxMotorTorque(0) {} - /// Constructor with limits but without motor + /// Constructor with limits but without motor with world-space anchor /** * @param rigidBody1 The first body of the joint * @param rigidBody2 The second body of the joint @@ -102,13 +118,14 @@ struct HingeJointInfo : public JointInfo { const Vector3& initRotationAxisWorld, decimal initMinAngleLimit, decimal initMaxAngleLimit) : JointInfo(rigidBody1, rigidBody2, JointType::HINGEJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), isMotorEnabled(false), minAngleLimit(initMinAngleLimit), maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxMotorTorque(0) {} - /// Constructor with limits and motor + /// Constructor with limits and motor with world-space anchor /** * @param rigidBody1 The first body of the joint * @param rigidBody2 The second body of the joint @@ -125,11 +142,95 @@ struct HingeJointInfo : public JointInfo { decimal initMinAngleLimit, decimal initMaxAngleLimit, decimal initMotorSpeed, decimal initMaxMotorTorque) : JointInfo(rigidBody1, rigidBody2, JointType::HINGEJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), isMotorEnabled(false), minAngleLimit(initMinAngleLimit), maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), maxMotorTorque(initMaxMotorTorque) {} + + /// Constructor without limits and without motor with local-space anchors + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param anchorPointBody1Local The initial anchor point on body 1 in local-space + * @param anchorPointBody2Local The initial anchor point on body 2 in local-space + * @param rotationBody1AxisLocal The initial rotation axis on body 1 in local-space + * @param rotationBody2AxisLocal The initial rotation axis on body 2 in local-space + */ + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1Local, + const Vector3& anchorPointBody2Local, + const Vector3& rotationBody1AxisLocal, + const Vector3& rotationBody2AxisLocal) + : JointInfo(rigidBody1, rigidBody2, JointType::HINGEJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1Local), + anchorPointBody2LocalSpace(anchorPointBody2Local), + rotationAxisBody1Local(rotationBody1AxisLocal), + rotationAxisBody2Local(rotationBody2AxisLocal), + isLimitEnabled(false), + isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), + motorSpeed(0), maxMotorTorque(0) {} + + /// Constructor with limits but without motor with local-space anchors + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param anchorPointBody1Local The initial anchor point on body 1 in local-space + * @param anchorPointBody2Local The initial anchor point on body 2 in local-space + * @param rotationBody1AxisLocal The initial rotation axis on body 1 in local-space + * @param rotationBody2AxisLocal The initial rotation axis on body 2 in local-space + * @param initMinAngleLimit The initial minimum limit angle (in radian) + * @param initMaxAngleLimit The initial maximum limit angle (in radian) + */ + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1Local, + const Vector3& anchorPointBody2Local, + const Vector3& rotationBody1AxisLocal, + const Vector3& rotationBody2AxisLocal, + decimal initMinAngleLimit, decimal initMaxAngleLimit) + : JointInfo(rigidBody1, rigidBody2, JointType::HINGEJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1Local), + anchorPointBody2LocalSpace(anchorPointBody2Local), + rotationAxisBody1Local(rotationBody1AxisLocal), + rotationAxisBody2Local(rotationBody2AxisLocal), + isLimitEnabled(true), + isMotorEnabled(false), minAngleLimit(initMinAngleLimit), + maxAngleLimit(initMaxAngleLimit), motorSpeed(0), + maxMotorTorque(0) {} + + /// Constructor with limits and motor with local-space anchors + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param anchorPointBody1Local The initial anchor point on body 1 in local-space + * @param anchorPointBody2Local The initial anchor point on body 2 in local-space + * @param rotationBody1AxisLocal The initial rotation axis on body 1 in local-space + * @param rotationBody2AxisLocal The initial rotation axis on body 2 in local-space + * @param initMinAngleLimit The initial minimum limit angle (in radian) + * @param initMaxAngleLimit The initial maximum limit angle (in radian) + * @param initMotorSpeed The initial motor speed of the joint (in radian per second) + * @param initMaxMotorTorque The initial maximum motor torque (in Newtons) + */ + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1Local, + const Vector3& anchorPointBody2Local, + const Vector3& rotationBody1AxisLocal, + const Vector3& rotationBody2AxisLocal, + decimal initMinAngleLimit, decimal initMaxAngleLimit, + decimal initMotorSpeed, decimal initMaxMotorTorque) + : JointInfo(rigidBody1, rigidBody2, JointType::HINGEJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1Local), + anchorPointBody2LocalSpace(anchorPointBody2Local), + rotationAxisBody1Local(rotationBody1AxisLocal), + rotationAxisBody2Local(rotationBody2AxisLocal), + isLimitEnabled(true), + isMotorEnabled(false), minAngleLimit(initMinAngleLimit), + maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), + maxMotorTorque(initMaxMotorTorque) {} }; // Class HingeJoint diff --git a/include/reactphysics3d/constraint/SliderJoint.h b/include/reactphysics3d/constraint/SliderJoint.h index c624bbcd..f9f4830f 100644 --- a/include/reactphysics3d/constraint/SliderJoint.h +++ b/include/reactphysics3d/constraint/SliderJoint.h @@ -47,12 +47,24 @@ struct SliderJointInfo : public JointInfo { // -------------------- Attributes -------------------- // + /// True if this object has been constructed using local-space anchors + bool isUsingLocalSpaceAnchors; + /// Anchor point (in world-space coordinates) Vector3 anchorPointWorldSpace; + /// Anchor point on body 1 (in local-space coordinates) + Vector3 anchorPointBody1LocalSpace; + + /// Anchor point on body 2 (in local-space coordinates) + Vector3 anchorPointBody2LocalSpace; + /// Slider axis (in world-space coordinates) Vector3 sliderAxisWorldSpace; + /// Hinge slider axis of body 1 (in local-space coordinates) + Vector3 sliderAxisBody1Local; + /// True if the slider limits are enabled bool isLimitEnabled; @@ -71,7 +83,7 @@ struct SliderJointInfo : public JointInfo { /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed decimal maxMotorForce; - /// Constructor without limits and without motor + /// Constructor without limits and without motor with world-space anchor /** * @param rigidBody1 The first body of the joint * @param rigidBody2 The second body of the joint @@ -82,12 +94,13 @@ struct SliderJointInfo : public JointInfo { const Vector3& initAnchorPointWorldSpace, const Vector3& initSliderAxisWorldSpace) : JointInfo(rigidBody1, rigidBody2, JointType::SLIDERJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} - /// Constructor with limits and no motor + /// Constructor with limits and no motor with world-space anchor /** * @param rigidBody1 The first body of the joint * @param rigidBody2 The second body of the joint @@ -101,6 +114,7 @@ struct SliderJointInfo : public JointInfo { const Vector3& initSliderAxisWorldSpace, decimal initMinTranslationLimit, decimal initMaxTranslationLimit) : JointInfo(rigidBody1, rigidBody2, JointType::SLIDERJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), isLimitEnabled(true), isMotorEnabled(false), @@ -108,7 +122,7 @@ struct SliderJointInfo : public JointInfo { maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0), maxMotorForce(0) {} - /// Constructor with limits and motor + /// Constructor with limits and motor with world-space anchor /** * @param rigidBody1 The first body of the joint * @param rigidBody2 The second body of the joint @@ -125,12 +139,86 @@ struct SliderJointInfo : public JointInfo { decimal initMinTranslationLimit, decimal initMaxTranslationLimit, decimal initMotorSpeed, decimal initMaxMotorForce) : JointInfo(rigidBody1, rigidBody2, JointType::SLIDERJOINT), + isUsingLocalSpaceAnchors(false), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), isLimitEnabled(true), isMotorEnabled(true), minTranslationLimit(initMinTranslationLimit), maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed), maxMotorForce(initMaxMotorForce) {} + + /// Constructor without limits and without motor with local-space anchor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param anchorPointBody1Local The initial anchor point on body 1 in local-space + * @param anchorPointBody2Local The initial anchor point on body 2 in local-space + * @param sliderAxisBody1Local The initial slider axis in body 1 local-space + */ + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1Local, + const Vector3& anchorPointBody2Local, + const Vector3& sliderAxisBody1Local) + : JointInfo(rigidBody1, rigidBody2, JointType::SLIDERJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1Local), + anchorPointBody2LocalSpace(anchorPointBody2Local), + sliderAxisBody1Local(sliderAxisBody1Local), + isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), + maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} + + /// Constructor with limits and no motor with local-space anchor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param anchorPointBody1Local The initial anchor point on body 1 in local-space + * @param anchorPointBody2Local The initial anchor point on body 2 in local-space + * @param sliderAxisBody1Local The initial slider axis in body 1 local-space + * @param initMinTranslationLimit The initial minimum translation limit (in meters) + * @param initMaxTranslationLimit The initial maximum translation limit (in meters) + */ + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1Local, + const Vector3& anchorPointBody2Local, + const Vector3& sliderAxisBody1Local, + decimal initMinTranslationLimit, decimal initMaxTranslationLimit) + : JointInfo(rigidBody1, rigidBody2, JointType::SLIDERJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1Local), + anchorPointBody2LocalSpace(anchorPointBody2Local), + sliderAxisBody1Local(sliderAxisBody1Local), + isLimitEnabled(true), isMotorEnabled(false), + minTranslationLimit(initMinTranslationLimit), + maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0), + maxMotorForce(0) {} + + /// Constructor with limits and motor with local-space anchor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param anchorPointBody1Local The initial anchor point on body 1 in local-space + * @param anchorPointBody2Local The initial anchor point on body 2 in local-space + * @param sliderAxisBody1Local The initial slider axis in body 1 local-space + * @param initMinTranslationLimit The initial minimum translation limit (in meters) + * @param initMaxTranslationLimit The initial maximum translation limit (in meters) + * @param initMotorSpeed The initial speed of the joint motor (in meters per second) + * @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) + */ + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& anchorPointBody1Local, + const Vector3& anchorPointBody2Local, + const Vector3& sliderAxisBody1Local, + decimal initMinTranslationLimit, decimal initMaxTranslationLimit, + decimal initMotorSpeed, decimal initMaxMotorForce) + : JointInfo(rigidBody1, rigidBody2, JointType::SLIDERJOINT), + isUsingLocalSpaceAnchors(true), + anchorPointBody1LocalSpace(anchorPointBody1Local), + anchorPointBody2LocalSpace(anchorPointBody2Local), + sliderAxisBody1Local(sliderAxisBody1Local), + isLimitEnabled(true), isMotorEnabled(true), + minTranslationLimit(initMinTranslationLimit), + maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed), + maxMotorForce(initMaxMotorForce) {} }; // Class SliderJoint diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h index 2d7d2f8d..065f2a66 100644 --- a/include/reactphysics3d/engine/PhysicsWorld.h +++ b/include/reactphysics3d/engine/PhysicsWorld.h @@ -141,7 +141,6 @@ class PhysicsWorld { defaultSleepAngularVelocity = decimal(3.0) * (PI / decimal(180.0)); nbMaxContactManifolds = 3; cosAngleSimilarContactManifold = decimal(0.95); - } ~WorldSettings() = default; diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 48eb787f..fba5cc09 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -38,13 +38,27 @@ const decimal BallAndSocketJoint::BETA = decimal(0.2); BallAndSocketJoint::BallAndSocketJoint(Entity entity, PhysicsWorld& world, const BallAndSocketJointInfo& jointInfo) : Joint(entity, world) { - // Get the transforms of the two bodies - const Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); - const Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); + Vector3 anchorPointBody1LocalSpace; + Vector3 anchorPointBody2LocalSpace; + + if (jointInfo.isUsingLocalSpaceAnchors) { + + anchorPointBody1LocalSpace = jointInfo.anchorPointBody1LocalSpace; + anchorPointBody2LocalSpace = jointInfo.anchorPointBody2LocalSpace; + } + else { + + // Get the transforms of the two bodies + const Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + const Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); + + anchorPointBody1LocalSpace = body1Transform.getInverse() * jointInfo.anchorPointWorldSpace; + anchorPointBody2LocalSpace = body2Transform.getInverse() * jointInfo.anchorPointWorldSpace; + } // Compute the local-space anchor point for each body - mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody1(entity, body1Transform.getInverse() * jointInfo.anchorPointWorldSpace); - mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody2(entity, body2Transform.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody1(entity, anchorPointBody1LocalSpace); + mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody2(entity, anchorPointBody2LocalSpace); } diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 83b80e3b..5072d16c 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -32,15 +32,30 @@ using namespace reactphysics3d; // Constructor -FixedJoint::FixedJoint(Entity entity, PhysicsWorld &world, const FixedJointInfo& jointInfo) +FixedJoint::FixedJoint(Entity entity, PhysicsWorld& world, const FixedJointInfo& jointInfo) : Joint(entity, world) { - // Compute the local-space anchor point for each body + + Vector3 anchorPointBody1LocalSpace; + Vector3 anchorPointBody2LocalSpace; + const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); - mWorld.mFixedJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); - mWorld.mFixedJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); + if (jointInfo.isUsingLocalSpaceAnchors) { + + anchorPointBody1LocalSpace = jointInfo.anchorPointBody1LocalSpace; + anchorPointBody2LocalSpace = jointInfo.anchorPointBody2LocalSpace; + } + else { + + // Compute the local-space anchor point for each body + anchorPointBody1LocalSpace = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + anchorPointBody2LocalSpace = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + } + + mWorld.mFixedJointsComponents.setLocalAnchorPointBody1(mEntity, anchorPointBody1LocalSpace); + mWorld.mFixedJointsComponents.setLocalAnchorPointBody2(mEntity, anchorPointBody2LocalSpace); // Store inverse of initial rotation from body 1 to body 2 in body 1 space: // diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index f6523908..624ed373 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -34,22 +34,43 @@ using namespace reactphysics3d; // Constructor HingeJoint::HingeJoint(Entity entity, PhysicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world) { + Vector3 anchorPointBody1Local; + Vector3 anchorPointBody2Local; + Vector3 hingeLocalAxisBody1; + Vector3 hingeLocalAxisBody2; + + const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); + + if (jointInfo.isUsingLocalSpaceAnchors) { + + anchorPointBody1Local = jointInfo.anchorPointBody1LocalSpace; + anchorPointBody2Local = jointInfo.anchorPointBody2LocalSpace; + + hingeLocalAxisBody1 = jointInfo.rotationAxisBody1Local; + hingeLocalAxisBody2 = jointInfo.rotationAxisBody2Local; + } + else { + + // Compute the local-space anchor point for each body + anchorPointBody1Local = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + anchorPointBody2Local = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the local-space hinge axis + hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + hingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + hingeLocalAxisBody1.normalize(); + hingeLocalAxisBody2.normalize(); + } + const decimal lowerLimit = mWorld.mHingeJointsComponents.getLowerLimit(mEntity); const decimal upperLimit = mWorld.mHingeJointsComponents.getUpperLimit(mEntity); assert(lowerLimit <= decimal(0) && lowerLimit >= decimal(-2.0) * PI); assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0) * PI); - // Compute the local-space anchor point for each body - const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); - const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); - mWorld.mHingeJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); - mWorld.mHingeJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mHingeJointsComponents.setLocalAnchorPointBody1(mEntity, anchorPointBody1Local); + mWorld.mHingeJointsComponents.setLocalAnchorPointBody2(mEntity, anchorPointBody2Local); - // Compute the local-space hinge axis - Vector3 hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - Vector3 hingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - hingeLocalAxisBody1.normalize(); - hingeLocalAxisBody2.normalize(); mWorld.mHingeJointsComponents.setHingeLocalAxisBody1(mEntity, hingeLocalAxisBody1); mWorld.mHingeJointsComponents.setHingeLocalAxisBody2(mEntity, hingeLocalAxisBody2); diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 7cc13a31..d22d6ecb 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -35,20 +35,47 @@ using namespace reactphysics3d; const decimal SliderJoint::BETA = decimal(0.2); // Constructor -SliderJoint::SliderJoint(Entity entity, PhysicsWorld &world, const SliderJointInfo& jointInfo) +SliderJoint::SliderJoint(Entity entity, PhysicsWorld& world, const SliderJointInfo& jointInfo) : Joint(entity, world) { assert(mWorld.mSliderJointsComponents.getUpperLimit(mEntity) >= decimal(0.0)); assert(mWorld.mSliderJointsComponents.getLowerLimit(mEntity) <= decimal(0.0)); assert(mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity) >= decimal(0.0)); - // Compute the local-space anchor point for each body + Vector3 anchorPointBody1Local; + Vector3 anchorPointBody2Local; + Vector3 sliderLocalAxisBody1; + Vector3 sliderLocalAxisBody2; + const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); - mWorld.mSliderJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); - mWorld.mSliderJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); - // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + if (jointInfo.isUsingLocalSpaceAnchors) { + + anchorPointBody1Local = jointInfo.anchorPointBody1LocalSpace; + anchorPointBody2Local = jointInfo.anchorPointBody2LocalSpace; + + sliderLocalAxisBody1 = jointInfo.sliderAxisBody1Local; + } + else { + + // Compute the local-space anchor point for each body + anchorPointBody1Local = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + anchorPointBody2Local = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the slider axis in local-space of body 1 + // TODO : Do not compute the inverse here, it has already been computed above + sliderLocalAxisBody1 = transform1.getOrientation().getInverse() * + jointInfo.sliderAxisWorldSpace; + sliderLocalAxisBody1.normalize(); + } + + mWorld.mSliderJointsComponents.setLocalAnchorPointBody1(mEntity, anchorPointBody1Local); + mWorld.mSliderJointsComponents.setLocalAnchorPointBody2(mEntity, anchorPointBody2Local); + + mWorld.mSliderJointsComponents.setSliderAxisBody1(mEntity, sliderLocalAxisBody1); + + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: // // q20 = q10 r0 // <=> r0 = q10^-1 q20 @@ -61,13 +88,6 @@ SliderJoint::SliderJoint(Entity entity, PhysicsWorld &world, const SliderJointIn // r0 = initial rotation rotation from body 1 to body 2 // TODO : Do not compute the inverse here, it has already been computed above mWorld.mSliderJointsComponents.setInitOrientationDifferenceInv(mEntity, transform2.getOrientation().getInverse() * transform1.getOrientation()); - - // Compute the slider axis in local-space of body 1 - // TODO : Do not compute the inverse here, it has already been computed above - Vector3 sliderAxisBody1 = transform1.getOrientation().getInverse() * - jointInfo.sliderAxisWorldSpace; - sliderAxisBody1.normalize(); - mWorld.mSliderJointsComponents.setSliderAxisBody1(mEntity, sliderAxisBody1); } // Enable/Disable the limits of the joint