diff --git a/include/reactphysics3d/constraint/HingeJoint.h b/include/reactphysics3d/constraint/HingeJoint.h index 10b0c590..f673b3af 100644 --- a/include/reactphysics3d/constraint/HingeJoint.h +++ b/include/reactphysics3d/constraint/HingeJoint.h @@ -210,6 +210,9 @@ class HingeJoint : public Joint { /// Return the intensity of the current torque applied for the joint motor decimal getMotorTorque(decimal timeStep) const; + /// Return the current hinge angle + decimal getAngle() const; + /// Return a string representation virtual std::string to_string() const override; }; diff --git a/include/reactphysics3d/systems/ConstraintSolverSystem.h b/include/reactphysics3d/systems/ConstraintSolverSystem.h index 54021aaa..9d574e0e 100644 --- a/include/reactphysics3d/systems/ConstraintSolverSystem.h +++ b/include/reactphysics3d/systems/ConstraintSolverSystem.h @@ -216,6 +216,9 @@ class ConstraintSolverSystem { #endif + // ---------- Friendship ---------- + + friend class HingeJoint; }; #ifdef IS_RP3D_PROFILING_ENABLED diff --git a/include/reactphysics3d/systems/SolveHingeJointSystem.h b/include/reactphysics3d/systems/SolveHingeJointSystem.h index 05e5f059..aa5342e4 100644 --- a/include/reactphysics3d/systems/SolveHingeJointSystem.h +++ b/include/reactphysics3d/systems/SolveHingeJointSystem.h @@ -92,8 +92,7 @@ class SolveHingeJointSystem { decimal upperLimitAngle) const; /// Compute the current angle around the hinge axis - decimal computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1, - const Quaternion& orientationBody2); + decimal computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1, const Quaternion& orientationBody2); public : @@ -133,6 +132,10 @@ class SolveHingeJointSystem { #endif + // ---------- Friendship ---------- + + friend class HingeJoint; + }; #ifdef IS_RP3D_PROFILING_ENABLED diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index f6523908..5f18c79c 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -207,7 +207,7 @@ decimal HingeJoint::getMaxAngleLimit() const { /** * @return The current speed of the joint motor (in radian per second) */ - decimal HingeJoint::getMotorSpeed() const { +decimal HingeJoint::getMotorSpeed() const { return mWorld.mHingeJointsComponents.getMotorSpeed(mEntity); } @@ -215,7 +215,7 @@ decimal HingeJoint::getMaxAngleLimit() const { /** * @return The maximum torque of the joint motor (in Newtons) */ - decimal HingeJoint::getMaxMotorTorque() const { +decimal HingeJoint::getMaxMotorTorque() const { return mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity); } @@ -224,10 +224,27 @@ decimal HingeJoint::getMaxAngleLimit() const { * @param timeStep The current time step (in seconds) * @return The intensity of the current torque (in Newtons) of the joint motor */ - decimal HingeJoint::getMotorTorque(decimal timeStep) const { +decimal HingeJoint::getMotorTorque(decimal timeStep) const { return mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) / timeStep; } +// Return the current hinge angle +/** + * @return The current hinge angle (in radians) + */ +decimal HingeJoint::getAngle() const { + + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + const Quaternion& orientationBody1 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation(); + const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); + + // Compute the current angle around the hinge axis + return mWorld.mConstraintSolverSystem.mSolveHingeJointSystem.computeCurrentHingeAngle(mEntity, orientationBody1, orientationBody2); +} + // Return the number of bytes used by the joint size_t HingeJoint::getSizeInBytes() const { return sizeof(HingeJoint);