204 lines
10 KiB
C++
204 lines
10 KiB
C++
/** @file
|
|
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
* @author Daniel CHAPPUIS
|
|
* @author Edouard DUPIN
|
|
* @copyright 2010-2016, Daniel Chappuis
|
|
* @copyright 2017, Edouard DUPIN
|
|
* @license MPL v2.0 (see license file)
|
|
*/
|
|
#pragma once
|
|
|
|
#include <ephysics/mathematics/mathematics.hpp>
|
|
#include <ephysics/engine/ConstraintSolver.hpp>
|
|
|
|
namespace ephysics {
|
|
/**
|
|
* This structure is used to gather the information needed to create a slider
|
|
* joint. This structure will be used to create the actual slider joint.
|
|
*/
|
|
struct SliderJointInfo : public JointInfo {
|
|
|
|
public :
|
|
vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
|
|
vec3 sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
|
|
boolean isLimitEnabled; //!< True if the slider limits are enabled
|
|
boolean isMotorEnabled; //!< True if the slider motor is enabled
|
|
float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
|
|
float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled
|
|
float motorSpeed; //!< Motor speed
|
|
float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
|
|
/**
|
|
* @brief Constructor without limits and without motor
|
|
* @param[in] rigidBody1 The first body of the joint
|
|
* @param[in] rigidBody2 The second body of the joint
|
|
* @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
|
|
* @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
|
|
*/
|
|
SliderJointInfo(RigidBody* rigidBody1,
|
|
RigidBody* rigidBody2,
|
|
vec3 initAnchorPointWorldSpace,
|
|
vec3 initSliderAxisWorldSpace):
|
|
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
|
this.anchorPointWorldSpace(initAnchorPointWorldSpace),
|
|
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
|
isLimitEnabled(false),
|
|
isMotorEnabled(false),
|
|
minTranslationLimit(-1.0),
|
|
maxTranslationLimit(1.0),
|
|
motorSpeed(0),
|
|
maxMotorForce(0) {
|
|
|
|
}
|
|
/**
|
|
* @brief Constructor with limits and no motor
|
|
* @param[in] rigidBody1 The first body of the joint
|
|
* @param[in] rigidBody2 The second body of the joint
|
|
* @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
|
|
* @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
|
|
* @param[in] initMinTranslationLimit The initial minimum translation limit (in meters)
|
|
* @param[in] initMaxTranslationLimit The initial maximum translation limit (in meters)
|
|
*/
|
|
SliderJointInfo(RigidBody* rigidBody1,
|
|
RigidBody* rigidBody2,
|
|
vec3 initAnchorPointWorldSpace,
|
|
vec3 initSliderAxisWorldSpace,
|
|
float initMinTranslationLimit,
|
|
float initMaxTranslationLimit):
|
|
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
|
this.anchorPointWorldSpace(initAnchorPointWorldSpace),
|
|
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
|
isLimitEnabled(true),
|
|
isMotorEnabled(false),
|
|
minTranslationLimit(initMinTranslationLimit),
|
|
maxTranslationLimit(initMaxTranslationLimit),
|
|
motorSpeed(0),
|
|
maxMotorForce(0) {
|
|
|
|
}
|
|
/**
|
|
* @brief Constructor with limits and motor
|
|
* @param[in] rigidBody1 The first body of the joint
|
|
* @param[in] rigidBody2 The second body of the joint
|
|
* @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
|
|
* @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
|
|
* @param[in] initMinTranslationLimit The initial minimum translation limit (in meters)
|
|
* @param[in] initMaxTranslationLimit The initial maximum translation limit (in meters)
|
|
* @param[in] initMotorSpeed The initial speed of the joint motor (in meters per second)
|
|
* @param[in] initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
|
|
*/
|
|
SliderJointInfo(RigidBody* rigidBody1,
|
|
RigidBody* rigidBody2,
|
|
vec3 initAnchorPointWorldSpace,
|
|
vec3 initSliderAxisWorldSpace,
|
|
float initMinTranslationLimit,
|
|
float initMaxTranslationLimit,
|
|
float initMotorSpeed,
|
|
float initMaxMotorForce):
|
|
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
|
this.anchorPointWorldSpace(initAnchorPointWorldSpace),
|
|
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
|
isLimitEnabled(true),
|
|
isMotorEnabled(true),
|
|
minTranslationLimit(initMinTranslationLimit),
|
|
maxTranslationLimit(initMaxTranslationLimit),
|
|
motorSpeed(initMotorSpeed),
|
|
maxMotorForce(initMaxMotorForce) {
|
|
|
|
}
|
|
};
|
|
|
|
/**
|
|
* @brief This class represents a slider joint. This joint has a one degree of freedom.
|
|
* It only allows relative translation of the bodies along a single direction and no
|
|
* rotation.
|
|
*/
|
|
class SliderJoint: public Joint {
|
|
private:
|
|
static float BETA; //!< Beta value for the position correction bias factor
|
|
vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
|
|
vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
|
|
vec3 this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
|
|
etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
|
|
etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
|
|
etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
|
|
vec3 this.N1; //!< First vector orthogonal to the slider axis local-space of body 1
|
|
vec3 this.N2; //!< Second vector orthogonal to the slider axis and this.N1 in local-space of body 1
|
|
vec3 this.R1; //!< Vector r1 in world-space coordinates
|
|
vec3 this.R2; //!< Vector r2 in world-space coordinates
|
|
vec3 this.R2CrossN1; //!< Cross product of r2 and n1
|
|
vec3 this.R2CrossN2; //!< Cross product of r2 and n2
|
|
vec3 this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
|
|
vec3 this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
|
|
vec3 this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
|
|
vec3 this.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
|
|
vec2 this.bTranslation; //!< Bias of the 2 translation raints
|
|
vec3 this.bRotation; //!< Bias of the 3 rotation raints
|
|
float this.bLowerLimit; //!< Bias of the lower limit raint
|
|
float this.bUpperLimit; //!< Bias of the upper limit raint
|
|
etk::Matrix2x2 this.inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
|
|
etk::Matrix3x3 this.inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
|
|
float this.inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix)
|
|
float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
|
|
vec2 this.impulseTranslation; //!< Accumulated impulse for the 2 translation raints
|
|
vec3 this.impulseRotation; //!< Accumulated impulse for the 3 rotation raints
|
|
float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
|
|
float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
|
|
float this.impulseMotor; //!< Accumulated impulse for the motor
|
|
boolean this.isLimitEnabled; //!< True if the slider limits are enabled
|
|
boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled
|
|
vec3 this.sliderAxisWorld; //!< Slider axis in world-space coordinates
|
|
float this.lowerLimit; //!< Lower limit (minimum translation distance)
|
|
float this.upperLimit; //!< Upper limit (maximum translation distance)
|
|
boolean this.isLowerLimitViolated; //!< True if the lower limit is violated
|
|
boolean this.isUpperLimitViolated; //!< True if the upper limit is violated
|
|
float this.motorSpeed; //!< Motor speed (in m/s)
|
|
float this.maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
|
|
/// Private copy-ructor
|
|
SliderJoint( SliderJoint raint);
|
|
/// Private assignment operator
|
|
SliderJoint operator=( SliderJoint raint);
|
|
/// Reset the limits
|
|
void resetLimits();
|
|
sizet getSizeInBytes() override;
|
|
void initBeforeSolve( ConstraintSolverData raintSolverData) override;
|
|
void warmstart( ConstraintSolverData raintSolverData) override;
|
|
void solveVelocityConstraint( ConstraintSolverData raintSolverData) override;
|
|
void solvePositionConstraint( ConstraintSolverData raintSolverData) override;
|
|
public :
|
|
/// Constructor
|
|
SliderJoint( SliderJointInfo jointInfo);
|
|
/// Destructor
|
|
virtual ~SliderJoint();
|
|
/// Return true if the limits or the joint are enabled
|
|
boolean isLimitEnabled() ;
|
|
/// Return true if the motor of the joint is enabled
|
|
boolean isMotorEnabled() ;
|
|
/// Enable/Disable the limits of the joint
|
|
void enableLimit(boolean isLimitEnabled);
|
|
/// Enable/Disable the motor of the joint
|
|
void enableMotor(boolean isMotorEnabled);
|
|
/// Return the current translation value of the joint
|
|
float getTranslation() ;
|
|
/// Return the minimum translation limit
|
|
float getMinTranslationLimit() ;
|
|
/// Set the minimum translation limit
|
|
void setMinTranslationLimit(float lowerLimit);
|
|
/// Return the maximum translation limit
|
|
float getMaxTranslationLimit() ;
|
|
/// Set the maximum translation limit
|
|
void setMaxTranslationLimit(float upperLimit);
|
|
/// Return the motor speed
|
|
float getMotorSpeed() ;
|
|
/// Set the motor speed
|
|
void setMotorSpeed(float motorSpeed);
|
|
/// Return the maximum motor force
|
|
float getMaxMotorForce() ;
|
|
/// Set the maximum motor force
|
|
void setMaxMotorForce(float maxMotorForce);
|
|
/// Return the intensity of the current force applied for the joint motor
|
|
float getMotorForce(float timeStep) ;
|
|
};
|
|
|
|
|
|
}
|