mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-04 20:57:49 +00:00
683 lines
26 KiB
C++
683 lines
26 KiB
C++
#pragma once
|
|
|
|
#include "solver/SolverConfig.h"
|
|
#include "solver/ConstraintJacobian.h"
|
|
#include "solver/SolverBody.h"
|
|
#include "v8kernel/SimBody.h"
|
|
#include "v8world/RotateJoint.h"
|
|
|
|
#include "simd/simd.h"
|
|
|
|
namespace RBX
|
|
{
|
|
|
|
class PGSSolver;
|
|
class DebugSerializer;
|
|
|
|
//
|
|
// ConstraintVariables: inputs to the solver, initialized by the constraint interface
|
|
//
|
|
class ConstraintVariables
|
|
{
|
|
public:
|
|
void serialize( DebugSerializer& s ) const;
|
|
|
|
static RBX_SIMD_INLINE void setReaction( ConstraintVariables* _vars, const Vector3& _r )
|
|
{
|
|
_vars[0].reaction = _r.x;
|
|
_vars[1].reaction = _r.y;
|
|
_vars[2].reaction = _r.z;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void setReaction( ConstraintVariables* _vars, float x, float y )
|
|
{
|
|
_vars[0].reaction = x;
|
|
_vars[1].reaction = y;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void setImpulse( ConstraintVariables* _vars, const Vector3& _i )
|
|
{
|
|
_vars[0].impulse = _i.x;
|
|
_vars[1].impulse = _i.y;
|
|
_vars[2].impulse = _i.z;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void setImpulse( ConstraintVariables* _vars, float x, float y )
|
|
{
|
|
_vars[0].impulse = x;
|
|
_vars[1].impulse = y;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void setMinImpulses( ConstraintVariables* _vars, const Vector3& _min )
|
|
{
|
|
_vars[0].minImpulseValue = _min.x;
|
|
_vars[1].minImpulseValue = _min.y;
|
|
_vars[2].minImpulseValue = _min.z;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void setMinImpulses( ConstraintVariables* _vars, float x, float y )
|
|
{
|
|
_vars[0].minImpulseValue = x;
|
|
_vars[1].minImpulseValue = y;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void setMaxImpulses( ConstraintVariables* _vars, const Vector3& _max )
|
|
{
|
|
_vars[0].maxImpulseValue = _max.x;
|
|
_vars[1].maxImpulseValue = _max.y;
|
|
_vars[2].maxImpulseValue = _max.z;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void setMaxImpulses( ConstraintVariables* _vars, float x, float y )
|
|
{
|
|
_vars[0].maxImpulseValue = x;
|
|
_vars[1].maxImpulseValue = y;
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void gatherComponents( simd::v4f& _impulses, simd::v4f& _reactions, simd::v4f& _min, simd::v4f& _max, const ConstraintVariables& _vars0 )
|
|
{
|
|
_min = simd::splat< 0 >( simd::v4f( _vars0.v ) );
|
|
_max = simd::splat< 1 >( simd::v4f( _vars0.v ) );
|
|
_reactions = simd::splat< 2 >( simd::v4f( _vars0.v ) );
|
|
_impulses = simd::splat< 3 >( simd::v4f( _vars0.v ) );
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void gatherComponents( simd::v4f& _impulses, simd::v4f& _reactions, simd::v4f& _min, simd::v4f& _max, const ConstraintVariables& _vars0, const ConstraintVariables& _vars1 )
|
|
{
|
|
transpose2x4( _min, _max, _reactions, _impulses, simd::v4f(_vars0.v), simd::v4f(_vars1.v) );
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void gatherComponents( simd::v4f& _impulses, simd::v4f& _reactions, simd::v4f& _min, simd::v4f& _max, const ConstraintVariables& _vars0, const ConstraintVariables& _vars1, const ConstraintVariables& _vars2 )
|
|
{
|
|
transpose3x4( _min, _max, _reactions, _impulses, (simd::v4f)_vars0.v, (simd::v4f)_vars1.v, (simd::v4f)_vars2.v );
|
|
}
|
|
|
|
static RBX_SIMD_INLINE void gatherComponents( simd::v4f& _impulses, simd::v4f& _reactions, simd::v4f& _min, simd::v4f& _max, const ConstraintVariables& _vars0, const ConstraintVariables& _vars1, const ConstraintVariables& _vars2, const ConstraintVariables& _vars3 )
|
|
{
|
|
transpose( _min, _max, _reactions, _impulses, (simd::v4f)_vars0.v, (simd::v4f)_vars1.v, (simd::v4f)_vars2.v, (simd::v4f)_vars3.v );
|
|
}
|
|
|
|
// Values that must be set by the Constraint::buildEquation
|
|
// Inputs expected by the solver
|
|
union
|
|
{
|
|
struct
|
|
{
|
|
float minImpulseValue;
|
|
float maxImpulseValue;
|
|
|
|
// Constraint must set this to the desired reaction
|
|
float reaction;
|
|
|
|
// The Constraint must set this to the impulse computed in the previous frame or 0.0f if it is not available.
|
|
// This will contain the result.
|
|
float impulse;
|
|
};
|
|
simd::v4f_pod v;
|
|
};
|
|
};
|
|
|
|
//
|
|
// MovingRegression: Fit best 2nd degree curve to the last few data points
|
|
//
|
|
class MovingRegression
|
|
{
|
|
public:
|
|
MovingRegression()
|
|
{
|
|
lastPoint = 0.0f;
|
|
lastTangent = 0.0f;
|
|
lastCurvature = 0.0f;
|
|
confidence = 0.0f;
|
|
}
|
|
|
|
inline float testFitNextDataPointZeroOrder( float y ) const
|
|
{
|
|
float predicted = lastPoint;
|
|
return confidence * std::abs( y - predicted ) / ( std::max( std::abs( y ), std::abs( predicted ) ) + 0.00001f ) ;
|
|
}
|
|
|
|
inline float testFitNextDataPointFirstOrder( float y ) const
|
|
{
|
|
float predicted = lastPoint + lastTangent;
|
|
return confidence * std::abs( y - predicted ) / ( std::max( std::abs( y ), std::abs( predicted ) ) + 0.00001f ) ;
|
|
}
|
|
|
|
inline float testFitNextDataPointSecondOrder( float y ) const
|
|
{
|
|
float predicted = lastPoint + lastTangent + lastCurvature;
|
|
return confidence * std::abs( y - predicted ) / ( std::max( std::abs( y ), std::abs( predicted ) ) + 0.00001f ) ;
|
|
}
|
|
|
|
float predict( ) const
|
|
{
|
|
return lastPoint;
|
|
}
|
|
|
|
void addDataPoint( float y, float weight )
|
|
{
|
|
float newTangent = y - lastPoint;
|
|
float newCurvature = newTangent - lastTangent;
|
|
lastCurvature = newCurvature;
|
|
lastTangent = newTangent;
|
|
lastPoint = y;
|
|
confidence += 0.1f * ( 1.0f - confidence );
|
|
}
|
|
|
|
void serialize( DebugSerializer& s) const;
|
|
|
|
float confidence;
|
|
float lastPoint;
|
|
float lastTangent;
|
|
float lastCurvature;
|
|
};
|
|
|
|
//
|
|
// Cached values for each constraint equation
|
|
//
|
|
class ConstraintCache
|
|
{
|
|
public:
|
|
ConstraintCache():
|
|
velocityImpulse( 0.0f ),
|
|
velocityReaction( 0.0f ),
|
|
positionImpulse( 0.0f ),
|
|
positionReaction( 0.0f ),
|
|
// These need to be initialized to the values in SolverConfig!
|
|
velocitySor( 1.9f ),
|
|
positionSor( 1.9f ),
|
|
velocityCacheDamping( 1.0f ),
|
|
positionCacheDamping( 1.0f ) { }
|
|
|
|
void cache( const ConstraintVariables& _velocityStage, const ConstraintVariables& _positionStage, float _sorVel, float _sorPos, bool _isCollision, const SolverConfig& config );
|
|
|
|
inline void readCache( ConstraintVariables& _velocityStage, ConstraintVariables& _positionStage, float& _sorVel, float& _sorPos ) const
|
|
{
|
|
_velocityStage.impulse = velocityImpulse;
|
|
_sorVel = velocitySor;
|
|
_velocityStage.reaction = velocityReaction;
|
|
_positionStage.impulse = positionImpulse;
|
|
_sorPos = positionSor;
|
|
_positionStage.reaction = positionReaction;
|
|
}
|
|
|
|
void serialize( DebugSerializer& s ) const;
|
|
|
|
float velocityImpulse;
|
|
float velocityReaction;
|
|
float velocitySor;
|
|
float velocityCacheDamping;
|
|
float positionImpulse;
|
|
float positionReaction;
|
|
float positionSor;
|
|
float positionCacheDamping;
|
|
|
|
MovingRegression velocityImpulseRegression;
|
|
MovingRegression positionImpulseRegression;
|
|
};
|
|
|
|
//
|
|
// Constraint definition: Base class for all constraints and collision classes
|
|
//
|
|
class Constraint
|
|
{
|
|
public:
|
|
enum Types
|
|
{
|
|
Types_Collision, // Special constraint type: only generated inside the solver from ContactConnectors
|
|
Types_Align2Axes,
|
|
Types_BallInSocket,
|
|
Types_AngularVelocity,
|
|
Types_LinearVelocity,
|
|
Types_AchievePosition,
|
|
Types_BodyAngularVelocity,
|
|
Types_LinearSpring,
|
|
Types_LegacyBreakableBallInSocket,
|
|
Types_LegacyAngularVelocity,
|
|
Types_Count
|
|
};
|
|
|
|
// Number of degrees of freedom constrained
|
|
inline unsigned getDimension() const { return dimensions; }
|
|
|
|
inline bool isBroken() const { return broken; }
|
|
|
|
// Read from the constraint cache, and call the overloaded build equation
|
|
// This should only be called by the solver
|
|
inline void restoreCacheAndBuildEquation(
|
|
ConstraintJacobianPair* _jacobian,
|
|
ConstraintVariables* _velocityStage,
|
|
ConstraintVariables* _positionStage,
|
|
float* _sorVel,
|
|
float* _sorPos,
|
|
boost::uint8_t* _useBlock,
|
|
const SolverBodyDynamicProperties& _bodyA,
|
|
const SolverBodyDynamicProperties& _bodyB,
|
|
const SolverConfig& _solverConfig,
|
|
float _dt );
|
|
|
|
// Write into the constraint cache
|
|
// This should only be called by the solver
|
|
inline void cache(
|
|
const ConstraintVariables* _velocityStage,
|
|
const ConstraintVariables* _positionStage,
|
|
const float* _sorVel, const float* _sorPos,
|
|
const SolverConfig& _config );
|
|
|
|
// Each breakable constraint will need to implement this, and return /true/ if the constraint changes state to broken
|
|
// This should only be called by the solver
|
|
inline void updateBrokenState(
|
|
const ConstraintVariables* _velocityStage,
|
|
const ConstraintVariables* _positionStage,
|
|
const SolverConfig& _config );
|
|
|
|
void setBodyA( Body* _a ) { bodyA = _a; }
|
|
void setBodyB( Body* _b ) { bodyB = _b; }
|
|
const Body* getBodyA() const { return bodyA; }
|
|
const Body* getBodyB() const { return bodyB; }
|
|
Body* getBodyA() { return bodyA; }
|
|
Body* getBodyB() { return bodyB; }
|
|
Types getType() const { return type; }
|
|
|
|
virtual ~Constraint();
|
|
|
|
void setUID( boost::uint64_t _index ) { uid = _index; }
|
|
bool hasValidUID() const { return uid != 0; }
|
|
boost::uint64_t getUID() const { return uid; }
|
|
|
|
// After the PGS has updated all i ts iterations, the last iteration reaction delta is passed in as parameter
|
|
enum Convergence
|
|
{
|
|
Convergence_Converges,
|
|
Convergence_Diverges,
|
|
Convergence_Undetermined
|
|
};
|
|
virtual Convergence testPGSConvergence( const float* _disp, const float* _residuals, const float* _deltaResiduals, const SolverConfig& _solverConfig ) { return Convergence_Converges; }
|
|
|
|
virtual void serialize( DebugSerializer& s ) const;
|
|
|
|
protected:
|
|
const ConstraintCache& getCache( unsigned d ) const { return cacheData[ d ]; }
|
|
ConstraintCache& getCache( unsigned d ) { return cacheData[ d ]; }
|
|
|
|
inline Constraint( Types _type, Body* _bodyA, Body* _bodyB, uint8_t _dimensions );
|
|
|
|
private:
|
|
// Main constraint interface to the solver
|
|
// Initializes the Jacobian and ConstraintVariables for the two stages
|
|
virtual void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) = 0;
|
|
|
|
// Each breakable constraint will need to implement this, and return /true/ if the constraint changes state to broken
|
|
virtual bool computeBrokenState(
|
|
const ConstraintVariables* _velocityStage,
|
|
const ConstraintVariables* _positionStage,
|
|
const SolverConfig& _config ) const { return false; }
|
|
|
|
// Disable copy constructs
|
|
Constraint( const Constraint& );
|
|
Constraint& operator=( const Constraint& );
|
|
|
|
protected:
|
|
uint8_t dimensions;
|
|
Types type : 8;
|
|
bool broken : 1;
|
|
private:
|
|
Body* bodyA;
|
|
Body* bodyB;
|
|
ConstraintCache* cacheData;
|
|
boost::uint64_t uid; // Current index if registered in the solver
|
|
};
|
|
|
|
//
|
|
// Constraint: inline implementation
|
|
//
|
|
inline Constraint::Constraint( Types _type, Body* _bodyA, Body* _bodyB, uint8_t _dimensions ): type( _type ), dimensions( _dimensions ), bodyA( _bodyA ), bodyB( _bodyB ), uid( 0 ), broken( false )
|
|
{
|
|
RBXASSERT( _bodyA != NULL );
|
|
cacheData = new ConstraintCache[ dimensions ];
|
|
}
|
|
|
|
#ifdef __RBX_NOT_RELEASE
|
|
static inline void checkConstraintVariables( const ConstraintVariables& _vars )
|
|
{
|
|
RBXASSERT( !RBX::Math::isNanInf( _vars.impulse ) );
|
|
RBXASSERT( !RBX::Math::isNanInf( _vars.reaction ) );
|
|
RBXASSERT( !RBX::Math::isNan( _vars.minImpulseValue ) );
|
|
RBXASSERT( !RBX::Math::isNan( _vars.maxImpulseValue ) );
|
|
}
|
|
|
|
static inline void checkJacobian( const ConstraintJacobianPair& _j )
|
|
{
|
|
RBXASSERT( !RBX::Math::isNanInfVector3( _j.a.lin ) );
|
|
RBXASSERT( !RBX::Math::isNanInfVector3( _j.b.lin ) );
|
|
RBXASSERT( !RBX::Math::isNanInfVector3( _j.a.ang ) );
|
|
RBXASSERT( !RBX::Math::isNanInfVector3( _j.b.ang ) );
|
|
}
|
|
#endif
|
|
|
|
RBX_SIMD_INLINE void Constraint::restoreCacheAndBuildEquation(
|
|
ConstraintJacobianPair* __restrict _jacobian,
|
|
ConstraintVariables* __restrict _varsVel,
|
|
ConstraintVariables* __restrict _varsPos,
|
|
float* __restrict _sorVel,
|
|
float* __restrict _sorPos,
|
|
boost::uint8_t* _useBlock,
|
|
const SolverBodyDynamicProperties& _bodyA,
|
|
const SolverBodyDynamicProperties& _bodyB,
|
|
const SolverConfig& _solverConfig,
|
|
float _dt )
|
|
{
|
|
for( unsigned i = 0; i < dimensions; i++ )
|
|
{
|
|
// Initialize to some reasonable default values
|
|
_varsVel[i].minImpulseValue = -std::numeric_limits<float>::infinity();
|
|
_varsVel[i].maxImpulseValue = std::numeric_limits<float>::infinity();
|
|
|
|
_varsPos[i].minImpulseValue = -std::numeric_limits<float>::infinity();
|
|
_varsPos[i].maxImpulseValue = std::numeric_limits<float>::infinity();
|
|
|
|
_jacobian[i].reset();
|
|
|
|
// Unless specified by the constraint, use the entire constraint as a Block in the Gauss-Seidel
|
|
_useBlock[i] = _solverConfig.blockPGSEnabled;
|
|
|
|
// Read previous frame impulse/SOR/reaction values
|
|
getCache(i).readCache(_varsVel[i],_varsPos[i], _sorVel[i], _sorPos[i]);
|
|
|
|
// Optionally disable the cache
|
|
_varsVel[i].impulse = _solverConfig.velCacheDamping * _varsVel[i].impulse;
|
|
_varsPos[i].impulse = _solverConfig.posCacheDamping * _varsPos[i].impulse;
|
|
}
|
|
|
|
buildEquation( _jacobian, _useBlock, _varsVel, _varsPos, _bodyA, _bodyB, _solverConfig, _dt );
|
|
|
|
// Run some sanity checks
|
|
#ifdef __RBX_NOT_RELEASE
|
|
for( unsigned i = 0; i < dimensions; i++ )
|
|
{
|
|
checkConstraintVariables( _varsVel[ i ] );
|
|
checkConstraintVariables( _varsPos[ i ] );
|
|
checkJacobian( _jacobian[ i ] );
|
|
}
|
|
#endif
|
|
}
|
|
|
|
RBX_SIMD_INLINE void Constraint::updateBrokenState(
|
|
const ConstraintVariables* _velocityStage,
|
|
const ConstraintVariables* _positionStage,
|
|
const SolverConfig& _config )
|
|
{
|
|
if( !broken )
|
|
{
|
|
broken = computeBrokenState(_velocityStage, _positionStage, _config);
|
|
}
|
|
}
|
|
|
|
inline void Constraint::cache( const ConstraintVariables* _velocityStage, const ConstraintVariables* _positionStage, const float* _sorVel, const float* _sorPos, const SolverConfig& _config )
|
|
{
|
|
// Cache Constraint base class data
|
|
for( unsigned i = 0; i < dimensions; i++ )
|
|
{
|
|
cacheData[ i ].cache( _velocityStage[ i ], _positionStage[ i ], _sorVel[ i ], _sorPos[ i ], getType() == Types_Collision, _config );
|
|
}
|
|
}
|
|
|
|
//
|
|
// ConstraintBallInSocket
|
|
//
|
|
class ConstraintBallInSocket: public Constraint
|
|
{
|
|
public:
|
|
ConstraintBallInSocket( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_BallInSocket, _bodyA, _bodyB, 3 ) { }
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
inline void setPivotA( const Vector3& _pivotA ) { pointA = _pivotA; }
|
|
inline void setPivotB( const Vector3& _pivotB ) { pointB = _pivotB; }
|
|
|
|
Convergence testPGSConvergence( const float* _disp, const float* _residuals, const float* _deltaResiduals, const SolverConfig& _solverConfig ) override;
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
private:
|
|
// Points on object A and B in object space, relative to center of mass
|
|
Vector3 pointA;
|
|
Vector3 pointB;
|
|
};
|
|
|
|
//
|
|
// ConstraintLegacyBreakableBallInSocket
|
|
//
|
|
class ConstraintLegacyBreakableBallInSocket: public Constraint
|
|
{
|
|
public:
|
|
ConstraintLegacyBreakableBallInSocket( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_LegacyBreakableBallInSocket, _bodyA, _bodyB, 3 ), pointA(0.0f), pointB(0.0f), broken( false ), maxNormalForce( std::numeric_limits<float>::infinity() )
|
|
{
|
|
setNormalOnA( Vector3(1.0f, 0.0f, 0.0f ) );
|
|
}
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void setPivotA( const Vector3& _pivotA ) { pointA = _pivotA; }
|
|
void setPivotB( const Vector3& _pivotB ) { pointB = _pivotB; }
|
|
void setNormalOnA( const Vector3& _normal );
|
|
void setMaxNormalForce( float _maxForce ) { maxNormalForce = _maxForce; }
|
|
bool computeBrokenState(
|
|
const ConstraintVariables* _velocityStage,
|
|
const ConstraintVariables* _positionStage,
|
|
const SolverConfig& _config ) const override;
|
|
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
private:
|
|
// Points on object A and B in object space, relative to center of mass
|
|
Vector3 pointA;
|
|
Vector3 pointB;
|
|
Vector3 normalA;
|
|
Vector3 tangentA1;
|
|
Vector3 tangentA2;
|
|
float maxNormalForce;
|
|
bool broken;
|
|
};
|
|
|
|
//
|
|
// ConstraintAlign2Axes
|
|
//
|
|
class ConstraintAlign2Axes: public Constraint
|
|
{
|
|
public:
|
|
ConstraintAlign2Axes( Body* _bodyA, Body* _bodyB );
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void setAxisA( const Vector3& a );
|
|
void setAxisB( const Vector3& b );
|
|
Vector3 getAxisA() const { return axisA; }
|
|
Vector3 getAxisB() const { return axisB; }
|
|
|
|
Convergence testPGSConvergence( const float* _disp, const float* _residuals, const float* _deltaResiduals, const SolverConfig& _solverConfig ) override;
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
private:
|
|
// Axis on body A in object space
|
|
Vector3 axisA;
|
|
|
|
// 2 normal axes to the axis on body B
|
|
// In object space
|
|
Vector3 axisB;
|
|
Vector3 orthogonalAxisB1;
|
|
Vector3 orthogonalAxisB2;
|
|
|
|
// Cache
|
|
Vector3 worldSpaceOrthogonalB1;
|
|
Vector3 worldSpaceOrthogonalB2;
|
|
};
|
|
|
|
//
|
|
// ConstraintAngularVelocity
|
|
//
|
|
class ConstraintAngularVelocity: public Constraint
|
|
{
|
|
public:
|
|
ConstraintAngularVelocity( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_AngularVelocity, _bodyA, _bodyB, 1 ),
|
|
maxForce( 0.0f ),
|
|
desiredAngularVelocity( 0.0f ) { }
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void setAxisA( const Vector3& a ) { axisA = a; }
|
|
void setAxisB( const Vector3& b ) { axisB = b; }
|
|
void setDesiredAngularVelocity( float _v ) { desiredAngularVelocity = _v; }
|
|
void setMaxForce( float _f ) { maxForce = _f; }
|
|
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
private:
|
|
Vector3 axisA;
|
|
Vector3 axisB;
|
|
float maxForce;
|
|
float desiredAngularVelocity;
|
|
};
|
|
|
|
//
|
|
// ConstraintLinearVelocity
|
|
//
|
|
class ConstraintLinearVelocity: public Constraint
|
|
{
|
|
public:
|
|
ConstraintLinearVelocity( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_LinearVelocity, _bodyA, _bodyB, 3 ), maxForce(0.0f), desiredVelocity(0.0f, 0.0f, 0.0f) { }
|
|
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void setDesiredVelocity( const Vector3& _v ) { desiredVelocity = _v; }
|
|
void setMaxForce( const Vector3& _f ) { maxForce = _f; }
|
|
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
private:
|
|
Vector3 maxForce;
|
|
Vector3 desiredVelocity;
|
|
};
|
|
|
|
class ConstraintLinearSpring: public Constraint
|
|
{
|
|
public:
|
|
ConstraintLinearSpring( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_LinearSpring, _bodyA, _bodyB, 3 ), pivotA(0.0f), pivotB(0.0f), maxForce(0.0f), p(0.0f), d(0.0f) { }
|
|
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void setPivotA( const Vector3& _pivotA ) { pivotA = _pivotA; }
|
|
void setPivotB( const Vector3& _pivotB ) { pivotB = _pivotB; }
|
|
void setMaxForce( const Vector3& _f ) { maxForce = _f; }
|
|
void setPD( float _p, float _d ) { p = _p; d = _d; }
|
|
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
private:
|
|
Vector3 pivotA;
|
|
Vector3 pivotB;
|
|
Vector3 maxForce;
|
|
float p, d;
|
|
};
|
|
|
|
class ConstraintAchievePosition: public Constraint
|
|
{
|
|
public:
|
|
ConstraintAchievePosition( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_AchievePosition, _bodyA, _bodyB, 3 ), maxForce( 0.0f ), targetVelocity( 0.0f ) { }
|
|
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void setPivotA( const Vector3& _pivotA ) { pivotA = _pivotA; }
|
|
void setPivotB( const Vector3& _pivotB ) { pivotB = _pivotB; }
|
|
void setTargetVelocity( const Vector3& _v ) { targetVelocity = _v; }
|
|
void setMaxForce( const Vector3& _f ) { maxForce = _f; }
|
|
void setMinForce( const Vector3& _f ) { minForce = _f; }
|
|
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
private:
|
|
Vector3 pivotA;
|
|
Vector3 pivotB;
|
|
Vector3 maxForce;
|
|
Vector3 minForce;
|
|
Vector3 targetVelocity;
|
|
};
|
|
|
|
class ConstraintBodyAngularVelocity: public Constraint
|
|
{
|
|
public:
|
|
ConstraintBodyAngularVelocity( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_BodyAngularVelocity, _bodyA, _bodyB, 3 ), targetAngularVelocity( 0.0f ), maxTorque( 0.0f ), minTorque( 0.0f ), useIntegratedVelocities( false ) { }
|
|
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
// The vector values need to be provided in object space of body A
|
|
void setTarget( const Vector3& v ) { targetAngularVelocity = v; }
|
|
void setMaxTorque( const Vector3& v ) { maxTorque = v; }
|
|
void setMinTorque( const Vector3& v ) { minTorque = v; }
|
|
void setUseIntegratedVelocities( bool flag ) { useIntegratedVelocities = flag; }
|
|
|
|
private:
|
|
Vector3 targetAngularVelocity;
|
|
Vector3 maxTorque;
|
|
Vector3 minTorque;
|
|
bool useIntegratedVelocities;
|
|
};
|
|
|
|
class ConstraintLegacyAngularVelocity: public Constraint
|
|
{
|
|
public:
|
|
ConstraintLegacyAngularVelocity( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_LegacyAngularVelocity, _bodyA, _bodyB, 3 ), targetAngularVelocity( 0.0f ), maxTorque( 0.0f ), minTorque( 0.0f ), useIntegratedVelocities( false ) { }
|
|
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
|
|
void serialize( DebugSerializer& s ) const override;
|
|
|
|
// The vector values need to be provided in object space of body A
|
|
void setTarget( const Vector3& v ) { targetAngularVelocity = v; }
|
|
void setMaxTorque( const Vector3& v ) { maxTorque = v; }
|
|
void setMinTorque( const Vector3& v ) { minTorque = v; }
|
|
void setUseIntegratedVelocities( bool flag ) { useIntegratedVelocities = flag; }
|
|
|
|
private:
|
|
Vector3 targetAngularVelocity;
|
|
Vector3 maxTorque;
|
|
Vector3 minTorque;
|
|
bool useIntegratedVelocities;
|
|
};
|
|
|
|
//
|
|
// ConstraintCollision
|
|
//
|
|
class ConstraintCollision: public Constraint
|
|
{
|
|
public:
|
|
ConstraintCollision( Body* _bodyA, Body* _bodyB ): Constraint( Constraint::Types_Collision, _bodyA, _bodyB, 3 )
|
|
{
|
|
cachedTangent1 = Vector3( 1.0f, 0.0f, 0.0f );
|
|
getCache(0).positionSor = 1.0f;
|
|
getCache(0).velocitySor = 1.0f;
|
|
getCache(1).positionSor = 1.0f;
|
|
getCache(1).velocitySor = 1.0f;
|
|
getCache(2).positionSor = 1.0f;
|
|
getCache(2).velocitySor = 1.0f;
|
|
}
|
|
|
|
void buildEquation( ConstraintJacobianPair* _jacobian, boost::uint8_t* _useBlock, ConstraintVariables* _velocityStage, ConstraintVariables* _positionStage, const SolverBodyDynamicProperties& _bodyA, const SolverBodyDynamicProperties& _bodyB, const SolverConfig& _config, float _dt ) override;
|
|
void setNormal( const Vector3& _normal ) { normal = _normal; }
|
|
void setPointA( const Vector3& _pointA ) { pointA = _pointA; }
|
|
void setDepth( float _d ) { depth = _d; }
|
|
void setFriction( float _f ) { friction = _f; }
|
|
void setResititution( float _r ) { restitution = _r; }
|
|
|
|
Convergence testPGSConvergence( const float* _disp, const float* _residuals, const float* _deltaResiduals, const SolverConfig& _solverConfig ) override;
|
|
void serialize( DebugSerializer& s ) const override;
|
|
private:
|
|
Vector3 normal;
|
|
Vector3 pointA;
|
|
float depth;
|
|
float friction;
|
|
float restitution;
|
|
|
|
// Cache
|
|
Vector3 cachedTangent1;
|
|
};
|
|
|
|
}
|