mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-05 05:07:48 +00:00
fahhh
This commit is contained in:
watrabi
@@ -0,0 +1,682 @@
|
||||
#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;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,274 @@
|
||||
#pragma once
|
||||
|
||||
#include "solver/SolverConfig.h"
|
||||
#include "solver/SolverContainers.h"
|
||||
#include "G3D/Vector3.h"
|
||||
|
||||
#include "boost/limits.hpp"
|
||||
#include "boost/math/constants/constants.hpp"
|
||||
#include "boost/container/vector.hpp"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "simd/simd.h"
|
||||
#include "rbx/ArrayDynamic.h"
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
class DebugSerializer;
|
||||
class ConstraintJacobianPair;
|
||||
|
||||
//
|
||||
// Use class definition rather than typedef so we can forward declare
|
||||
//
|
||||
class BodyPairIndices: public std::pair< int, int >
|
||||
{
|
||||
public:
|
||||
inline BodyPairIndices() { }
|
||||
inline BodyPairIndices( int a, int b ): std::pair< int, int >( a, b ) { }
|
||||
};
|
||||
|
||||
//
|
||||
// VirtualDisplacement: http://en.wikipedia.org/wiki/Virtual_displacement
|
||||
// A 6-dimensional vector with a linear part and angular part
|
||||
//
|
||||
|
||||
// This is a version that allows conversion from non-simd to simd types.
|
||||
// The non-simd is still used by integration.
|
||||
class VirtualDisplacementPOD
|
||||
{
|
||||
public:
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
union
|
||||
{
|
||||
simd::v4f_pod linV4;
|
||||
Vector3Pod lin;
|
||||
};
|
||||
union
|
||||
{
|
||||
simd::v4f_pod angV4;
|
||||
Vector3Pod ang;
|
||||
};
|
||||
};
|
||||
|
||||
// This is a version that can be worked with in simd
|
||||
// It's only ever used on the stack
|
||||
class VirtualDisplacement
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE VirtualDisplacement( ){ }
|
||||
RBX_SIMD_INLINE VirtualDisplacement( const simd::v4f& linear, const simd::v4f& angular ): lin( linear ), ang( angular ) { }
|
||||
RBX_SIMD_INLINE VirtualDisplacement( const VirtualDisplacementPOD& _v ): lin( _v.linV4 ), ang( _v.angV4 ) { }
|
||||
RBX_SIMD_INLINE operator VirtualDisplacementPOD()
|
||||
{
|
||||
VirtualDisplacementPOD r;
|
||||
r.linV4 = lin;
|
||||
r.angV4 = ang;
|
||||
return r;
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE void reset()
|
||||
{
|
||||
lin = simd::zerof();
|
||||
ang = simd::zerof();
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f getLin() const { return lin; }
|
||||
RBX_SIMD_INLINE simd::v4f getAng() const { return ang; }
|
||||
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
private:
|
||||
simd::v4f lin;
|
||||
simd::v4f ang;
|
||||
};
|
||||
|
||||
//
|
||||
// Array of virtual displacements
|
||||
//
|
||||
class VirtualDisplacementArray
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE VirtualDisplacementArray( size_t _size, size_t alignment ): size( _size ), data( _size, ArrayNoInit(), alignment )
|
||||
{ }
|
||||
|
||||
inline void reset()
|
||||
{
|
||||
VirtualDisplacement z( simd::zerof(), simd::zerof() );
|
||||
for( size_t i = 0; i < size; i++ )
|
||||
{
|
||||
data[ i ] = z;
|
||||
}
|
||||
}
|
||||
|
||||
const VirtualDisplacementPOD* getData() const { return data.data(); }
|
||||
VirtualDisplacementPOD* getData() { return data.data(); }
|
||||
RBX_SIMD_INLINE size_t getSize() const { return size; }
|
||||
|
||||
RBX_SIMD_INLINE VirtualDisplacementPOD operator[]( int i ) const
|
||||
{
|
||||
RBXASSERT_VERY_FAST( (size_t)i < size );
|
||||
return data[ i ];
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE VirtualDisplacementPOD& operator[]( int i )
|
||||
{
|
||||
RBXASSERT_VERY_FAST( (size_t)i < size );
|
||||
return data[ i ];
|
||||
}
|
||||
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
private:
|
||||
size_t size;
|
||||
ArrayDynamic< VirtualDisplacementPOD > data;
|
||||
};
|
||||
|
||||
//
|
||||
// Effective mass vectors: inverse mass matrix * jacobian
|
||||
//
|
||||
class EffectiveMass
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE EffectiveMass() { }
|
||||
RBX_SIMD_INLINE EffectiveMass( const simd::v4f& linear, const simd::v4f& angular ): lin( linear ), ang( angular ) { }
|
||||
RBX_SIMD_INLINE void applyMultiplier( const simd::v4f& m )
|
||||
{
|
||||
lin = m * lin;
|
||||
ang = m * ang;
|
||||
}
|
||||
RBX_SIMD_INLINE void reset()
|
||||
{
|
||||
lin = simd::zerof();
|
||||
ang = simd::zerof();
|
||||
}
|
||||
RBX_SIMD_INLINE simd::v4f getLin() const { return lin; }
|
||||
RBX_SIMD_INLINE simd::v4f getAng() const { return ang; }
|
||||
|
||||
private:
|
||||
simd::v4f lin;
|
||||
simd::v4f ang;
|
||||
};
|
||||
|
||||
class EffectiveMassPair
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE EffectiveMassPair( const EffectiveMass& _a, const EffectiveMass& _b ): a( _a ), b( _b ) { }
|
||||
|
||||
RBX_SIMD_INLINE EffectiveMassPair( ) { }
|
||||
|
||||
RBX_SIMD_INLINE void reset()
|
||||
{
|
||||
a.reset();
|
||||
b.reset();
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE void applyMultipliers( const simd::v4f& mA, const simd::v4f& mB )
|
||||
{
|
||||
a.applyMultiplier( mA );
|
||||
b.applyMultiplier( mB );
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f getLinA() const { return a.getLin(); }
|
||||
RBX_SIMD_INLINE simd::v4f getLinB() const { return b.getLin(); }
|
||||
RBX_SIMD_INLINE simd::v4f getAngA() const { return a.getAng(); }
|
||||
RBX_SIMD_INLINE simd::v4f getAngB() const { return b.getAng(); }
|
||||
|
||||
RBX_SIMD_INLINE EffectiveMass getPartA() const { return a; }
|
||||
RBX_SIMD_INLINE EffectiveMass getPartB() const { return b; }
|
||||
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
private:
|
||||
EffectiveMass a;
|
||||
EffectiveMass b;
|
||||
};
|
||||
|
||||
//
|
||||
// Jacobian of a binary constraint
|
||||
//
|
||||
class ConstraintJacobian
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE void reset()
|
||||
{
|
||||
linV4 = simd::zerof();
|
||||
angV4 = simd::zerof();
|
||||
}
|
||||
|
||||
union
|
||||
{
|
||||
Vector3Pod lin;
|
||||
simd::v4f_pod linV4;
|
||||
};
|
||||
union
|
||||
{
|
||||
Vector3Pod ang;
|
||||
simd::v4f_pod angV4;
|
||||
};
|
||||
};
|
||||
|
||||
class ConstraintJacobianPair
|
||||
{
|
||||
public:
|
||||
class LinA;
|
||||
class LinB;
|
||||
class AngA;
|
||||
class AngB;
|
||||
|
||||
template< class PartSelect >
|
||||
RBX_SIMD_INLINE simd::v4f get() const;
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f getLinA() const { return a.linV4; }
|
||||
RBX_SIMD_INLINE simd::v4f getLinB() const { return b.linV4; }
|
||||
RBX_SIMD_INLINE simd::v4f getAngA() const { return a.angV4; }
|
||||
RBX_SIMD_INLINE simd::v4f getAngB() const { return b.angV4; }
|
||||
|
||||
template< class PartSelect >
|
||||
RBX_SIMD_INLINE void set( const simd::v4f& v );
|
||||
|
||||
RBX_SIMD_INLINE void setLinA( const simd::v4f& v ) { a.linV4 = v; }
|
||||
RBX_SIMD_INLINE void setLinB( const simd::v4f& v ) { b.linV4 = v; }
|
||||
RBX_SIMD_INLINE void setAngA( const simd::v4f& v ) { a.angV4 = v; }
|
||||
RBX_SIMD_INLINE void setAngB( const simd::v4f& v ) { b.angV4 = v; }
|
||||
|
||||
RBX_SIMD_INLINE void reset()
|
||||
{
|
||||
a.reset();
|
||||
b.reset();
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f dot( const EffectiveMassPair& _v ) const
|
||||
{
|
||||
simd::v4f partA = getLinA() * _v.getLinA() + getAngA() * _v.getAngA();
|
||||
simd::v4f partB = getLinB() * _v.getLinB() + getAngB() * _v.getAngB();
|
||||
simd::v4f r = partA + partB;
|
||||
return simd::splat<0>(r) + simd::splat<1>(r) + simd::splat<2>(r);
|
||||
}
|
||||
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
ConstraintJacobian a;
|
||||
ConstraintJacobian b;
|
||||
};
|
||||
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f ConstraintJacobianPair::get< ConstraintJacobianPair::LinA >() const { return a.linV4; }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f ConstraintJacobianPair::get< ConstraintJacobianPair::LinB >() const { return b.linV4; }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f ConstraintJacobianPair::get< ConstraintJacobianPair::AngA >() const { return a.angV4; }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f ConstraintJacobianPair::get< ConstraintJacobianPair::AngB >() const { return b.angV4; }
|
||||
|
||||
template< >
|
||||
RBX_SIMD_INLINE void ConstraintJacobianPair::set< ConstraintJacobianPair::LinA >( const simd::v4f& v ) { a.linV4 = v; }
|
||||
template< >
|
||||
RBX_SIMD_INLINE void ConstraintJacobianPair::set< ConstraintJacobianPair::LinB >( const simd::v4f& v ) { b.linV4 = v; }
|
||||
template< >
|
||||
RBX_SIMD_INLINE void ConstraintJacobianPair::set< ConstraintJacobianPair::AngA >( const simd::v4f& v ) { a.angV4 = v; }
|
||||
template< >
|
||||
RBX_SIMD_INLINE void ConstraintJacobianPair::set< ConstraintJacobianPair::AngB >( const simd::v4f& v ) { b.angV4 = v; }
|
||||
|
||||
}
|
||||
@@ -0,0 +1,193 @@
|
||||
#pragma once
|
||||
|
||||
#include "util/G3DCore.h"
|
||||
#include "util/Quaternion.h"
|
||||
|
||||
#include "boost/type_traits.hpp"
|
||||
#include "boost/utility.hpp"
|
||||
#include "boost/cstdint.hpp"
|
||||
#include "boost/container/map.hpp"
|
||||
|
||||
#include "simd/simd.h"
|
||||
#include "rbx/ArrayDynamic.h"
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
class DebugSerializer;
|
||||
|
||||
// Compile time 'Has X Method' implementation using 'Substitution Failure is not an Error'
|
||||
template< typename T >
|
||||
struct HasSerializeMethod
|
||||
{
|
||||
private:
|
||||
template<typename U, void (U::*)( DebugSerializer& ) const> struct SFINAE {};
|
||||
template<typename U> static char Test(SFINAE<U, &U::serialize>*);
|
||||
template<typename U> static int Test(...);
|
||||
public:
|
||||
static const bool value = sizeof(Test<T>(0)) == sizeof(char);
|
||||
};
|
||||
|
||||
class DebugSerializerScope
|
||||
{
|
||||
public:
|
||||
DebugSerializerScope( DebugSerializer& s );
|
||||
~DebugSerializerScope();
|
||||
|
||||
private:
|
||||
DebugSerializer& serializer;
|
||||
size_t reservedBufferIndex;
|
||||
size_t currentSize;
|
||||
};
|
||||
|
||||
class DebugSerializer
|
||||
{
|
||||
public:
|
||||
void clear()
|
||||
{
|
||||
data.clear();
|
||||
}
|
||||
|
||||
template< class T >
|
||||
typename boost::enable_if< boost::is_arithmetic< T >, DebugSerializer >::type& storeAt( const T& t, size_t index )
|
||||
{
|
||||
union
|
||||
{
|
||||
T t;
|
||||
char bytes[ sizeof( T ) ];
|
||||
} u;
|
||||
u.t = t;
|
||||
for( size_t i = 0; i < sizeof( T ); i++ )
|
||||
{
|
||||
data[ index + i ] = u.bytes[ i ];
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T >
|
||||
typename boost::enable_if< boost::is_arithmetic< T >, DebugSerializer >::type& operator&( const T& t )
|
||||
{
|
||||
size_t index = data.size();
|
||||
data.resize( data.size() + sizeof( T ) );
|
||||
storeAt( t, index );
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T >
|
||||
typename boost::enable_if< boost::is_enum< T >, DebugSerializer >::type& operator&( const T& t )
|
||||
{
|
||||
union
|
||||
{
|
||||
T t;
|
||||
boost::uint8_t bytes[ sizeof( T ) ];
|
||||
} u;
|
||||
u.t = t;
|
||||
for( size_t i = 0; i < sizeof( T ); i++ )
|
||||
{
|
||||
data.push_back( u.bytes[ i ] );
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
DebugSerializer& operator&( const Vector3& v )
|
||||
{
|
||||
*this & v.x & v.y & v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
DebugSerializer& operator&( const Quaternion& q )
|
||||
{
|
||||
*this & q.x & q.y & q.z & q.w;
|
||||
return *this;
|
||||
}
|
||||
|
||||
DebugSerializer& operator&( const Matrix3& m )
|
||||
{
|
||||
*this & m.row( 0 ) & m.row( 1 ) & m.row( 2 );
|
||||
return *this;
|
||||
}
|
||||
|
||||
DebugSerializer& operator&( const simd::v4f& v )
|
||||
{
|
||||
*this & simd::extractSlow( v, 0 ) & simd::extractSlow( v, 1 ) & simd::extractSlow( v, 2 ) & simd::extractSlow( v, 3 );
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T, class U >
|
||||
DebugSerializer& operator&( const std::pair< T, U >& p )
|
||||
{
|
||||
*this & p.first & p.second;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T >
|
||||
DebugSerializer& operator&( const ArrayBase<T>& v )
|
||||
{
|
||||
*this & boost::uint32_t( v.size() );
|
||||
for( const auto& e : v )
|
||||
{
|
||||
*this & e;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T >
|
||||
DebugSerializer& operator&( const std::vector<T>& v )
|
||||
{
|
||||
*this & boost::uint32_t( v.size() );
|
||||
for( const auto& e : v )
|
||||
{
|
||||
*this & e;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T >
|
||||
typename boost::enable_if_c< HasSerializeMethod< T >::value && !boost::is_pointer< T >::value, DebugSerializer >::type& operator&( const T& t )
|
||||
{
|
||||
t.serialize( *this );
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T >
|
||||
typename boost::enable_if_c< HasSerializeMethod< T >::value, DebugSerializer >::type& operator&( const T* const t )
|
||||
{
|
||||
t->serialize( *this );
|
||||
return *this;
|
||||
}
|
||||
|
||||
DebugSerializer& tag( const char* name )
|
||||
{
|
||||
boost::uint8_t length = strlen(name);
|
||||
*this & length;
|
||||
for( boost::uint8_t i = 0; i < length; i++ )
|
||||
{
|
||||
*this & name[ i ];
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
std::vector< char > data;
|
||||
};
|
||||
|
||||
inline DebugSerializerScope::DebugSerializerScope( DebugSerializer& s ): serializer( s )
|
||||
{
|
||||
reservedBufferIndex = s.data.size();
|
||||
size_t size = 0;
|
||||
s & size;
|
||||
// size_t checkSum = 0;
|
||||
// s & checkSum;
|
||||
currentSize = s.data.size();
|
||||
}
|
||||
|
||||
inline DebugSerializerScope::~DebugSerializerScope()
|
||||
{
|
||||
size_t size = serializer.data.size() - currentSize;
|
||||
serializer.storeAt( size, reservedBufferIndex );
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,193 @@
|
||||
#pragma once
|
||||
|
||||
#include "solver/SolverConfig.h"
|
||||
#include "solver/Constraint.h"
|
||||
#include "solver/SolverContainers.h"
|
||||
#include "solver/SolverProfiler.h"
|
||||
#include "solver/SolverSerializer.h"
|
||||
|
||||
#include "v8kernel/SimBody.h"
|
||||
|
||||
#include "boost/unordered/unordered_map.hpp"
|
||||
#include "boost/unordered/unordered_set.hpp"
|
||||
#include "boost/container/vector.hpp"
|
||||
#include <map>
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
class ContactConnector;
|
||||
class RotateJoint;
|
||||
class ContactManifold;
|
||||
class Body;
|
||||
|
||||
typedef std::pair< boost::uint64_t, boost::uint64_t > BodyUIDPair;
|
||||
|
||||
class InconsistentBodyPair
|
||||
{
|
||||
public:
|
||||
bool operator<( const InconsistentBodyPair& pair ) const
|
||||
{
|
||||
return bodyPair < pair.bodyPair;
|
||||
}
|
||||
|
||||
Body* bodyA;
|
||||
Body* bodyB;
|
||||
BodyUIDPair bodyPair;
|
||||
Constraint::Convergence convergence;
|
||||
};
|
||||
|
||||
class OrderedConnector
|
||||
{
|
||||
public:
|
||||
ContactConnector* connector;
|
||||
bool swap;
|
||||
};
|
||||
|
||||
class BadConnector
|
||||
{
|
||||
public:
|
||||
Vector3 position;
|
||||
Constraint::Convergence convergence;
|
||||
};
|
||||
|
||||
class PGSSolver
|
||||
{
|
||||
public:
|
||||
PGSSolver();
|
||||
|
||||
// Add/remove SimBodies
|
||||
// High priority bodies will be simulated even if throttled
|
||||
void addSimBody( SimBody* body, bool highPriority );
|
||||
void removeSimBody( SimBody* body );
|
||||
|
||||
// Add/remove constraints
|
||||
void addConstraint( Constraint* _constraint );
|
||||
void removeConstraint( Constraint* _constraint );
|
||||
|
||||
// Solver
|
||||
// If throttled is set to true, only high priority bodies will be simulated.
|
||||
void solve( const std::vector< ContactConnector* >& connectors, float dt, boost::uint64_t debugTime, bool throttled );
|
||||
void solvePositions( const std::vector< ContactConnector* >& _contactConnectors );
|
||||
// Exactly like it was before the physics analyzer (sleeping islands) were submitted
|
||||
void solveLegacy( const std::vector< ContactConnector* >& _contactConnectors, float _dt, boost::uint64_t debugTime, bool _throttled );
|
||||
|
||||
// Create and delete contact manifolds
|
||||
// The parameters are uid's of both Object instances.
|
||||
void addContactManifold( boost::uint64_t _uidA, boost::uint64_t _uidB );
|
||||
void removeContactManifold( boost::uint64_t _uidA, boost::uint64_t _uidB );
|
||||
|
||||
// Clear body cache
|
||||
void clearBodyCache( boost::uint64_t _uid );
|
||||
|
||||
// Switch inconsistent constraint detector
|
||||
void setInconsistentConstraintDetectorEnabled( bool value ) { inconsistentConstraintDetectorEnabled = value; }
|
||||
void setPhysicsAnalyzerBreakOnIssue( bool value ) { physicsAnalyzerBreakOnIssue = value; }
|
||||
bool getPhysicsAnalyzerBreakOnIssue( ) const { return physicsAnalyzerBreakOnIssue; }
|
||||
|
||||
const ArrayBase< InconsistentBodyPair >& getInconsistentBodyPairs() const { return inconsistentBodyPairs; }
|
||||
const ArrayBase< ArrayDynamic< boost::uint64_t > >& getInconsistentBodies() const { return inconsistentBodies; }
|
||||
|
||||
void dumpLog( bool enable );
|
||||
void setUserId( int id ) { userId = id; }
|
||||
|
||||
private:
|
||||
void solveInternal( const std::vector< ContactConnector* >& connectors, float dt, boost::uint64_t debugTime, bool throttled, const SolverConfig& _solverConfig );
|
||||
void solveIsland( const ArrayDynamic< Constraint* >& constraints, const ArrayDynamic< SimBody* >& selectedSimBodies,
|
||||
float _dt, const SolverConfig& _solverConfig );
|
||||
ContactManifold* updateContactManifold( const BodyUIDPair& _pairId, const ArrayBase< OrderedConnector >& _manifold );
|
||||
size_t addContactConnectors( ArrayDynamic< ContactManifold* >& _activeManifolds, const std::vector< ContactConnector* >& _connectors, const boost::unordered_set< SimBody* >& _simBodies );
|
||||
void initAnchoredObjects(
|
||||
ArrayDynamic< SolverBodyDynamicProperties >& _bodyVariableData,
|
||||
ArrayDynamic< SolverBodyStaticProperties >& _bodyStaticData,
|
||||
ArrayDynamic< SolverBodyMassAndInertia >& _massAndInertia,
|
||||
ArrayDynamic< float >& _effectiveMassMultipliers,
|
||||
VirtualDisplacementArray& _velocityDeltas,
|
||||
VirtualDisplacementArray& _positionDeltas,
|
||||
int offsetToAnchoredObjects,
|
||||
const ArrayBase< SimBody* >& _anchoredBodyList,
|
||||
const SolverConfig& _config ) const;
|
||||
void integratePositionsAndUpdateSimBodies(
|
||||
SimBody* const * _simBodies,
|
||||
SolverBodyDynamicProperties* const _bodyVariableData,
|
||||
const SolverBodyStaticProperties* const _bodyStaticData,
|
||||
size_t _simBodyCount,
|
||||
const VirtualDisplacementArray& _velocityDeltas,
|
||||
const VirtualDisplacementArray& _positionDeltas,
|
||||
float _dt );
|
||||
void integratePositionsIgnoreVelocitiesAndUpdateSimBodies(
|
||||
SimBody* const * _simBodies,
|
||||
SolverBodyDynamicProperties* const _bodyVariableData,
|
||||
const SolverBodyStaticProperties* const _bodyStaticData,
|
||||
size_t _simBodyCount,
|
||||
const VirtualDisplacementArray& _positionDeltas,
|
||||
float _dt );
|
||||
void detectInconsistentConstraints(
|
||||
VirtualDisplacementArray& positionDeltasSIMD,
|
||||
ArrayBase< ConstraintVariables >& positionStage,
|
||||
const ArrayBase< ConstraintJacobianPair >& jacobians,
|
||||
const ArrayBase< ConstraintJacobianPair >& preconditionedJacobiansPosStage,
|
||||
const ArrayBase< EffectiveMassPair >& effectiveMassesPos,
|
||||
const ArrayBase< float >& sorPos,
|
||||
const ArrayBase< boost::uint8_t >& dimensions,
|
||||
const ArrayBase< boost::uint32_t >& offsets,
|
||||
const ArrayBase< BodyPairIndices >& simBodyPairs,
|
||||
const ArrayBase< Constraint* >& constraints,
|
||||
size_t collisionCount,
|
||||
const SolverConfig& solverConfig );
|
||||
|
||||
|
||||
boost::uint64_t constraintUIDGenerator;
|
||||
// Pure constraints - not including the Collision constraints
|
||||
SolverOrderedMap< boost::uint64_t, Constraint* >::Type pureConstraintSet;
|
||||
|
||||
SolverUnorderedMap< BodyUIDPair, ContactManifold* >::Type contactManifolds;
|
||||
|
||||
class SolverBodyCache
|
||||
{
|
||||
public:
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
Vector3 virDPosStageLin;
|
||||
Vector3 virDPosStageAng;
|
||||
Vector3 virDVelStageLin;
|
||||
Vector3 virDVelStageAng;
|
||||
Vector3 linearVelocity;
|
||||
Vector3 angularVelocity;
|
||||
Vector3 integratedLinearVelocity;
|
||||
Vector3 integratedAngularVelocity;
|
||||
SimBody* simBodyDebug;
|
||||
};
|
||||
|
||||
SolverUnorderedMap< boost::uint64_t, SolverBodyCache >::Type bodyCache;
|
||||
boost::unordered_set< SimBody* > simBodies;
|
||||
boost::unordered_set< SimBody* > highPrioritySimBodies;
|
||||
|
||||
// Inconsistent constraint detector
|
||||
bool inconsistentConstraintDetectorEnabled;
|
||||
bool physicsAnalyzerBreakOnIssue;
|
||||
ArrayDynamic< InconsistentBodyPair > inconsistentBodyPairs;
|
||||
ArrayDynamic< ArrayDynamic< boost::uint64_t > > inconsistentBodies;
|
||||
|
||||
// Serializer
|
||||
SolverSerializer serializer;
|
||||
|
||||
// Profilers
|
||||
SolverProfiler gatherCollisionsProfiler;
|
||||
SolverProfiler islandSplitProfiler;
|
||||
SolverProfiler integrateVelocitiesProfiler;
|
||||
SolverProfiler initAnchoredBodiesProfiler;
|
||||
SolverProfiler buildEquationsProfiler;
|
||||
SolverProfiler computeEffectiveMassesProfiler;
|
||||
SolverProfiler preconditioningProfiler;
|
||||
SolverProfiler multiplyEffectiveMassMultipliersProfiler;
|
||||
SolverProfiler initVirDProfiler;
|
||||
SolverProfiler kernelProfiler;
|
||||
SolverProfiler integratePositionsProfiler;
|
||||
SolverProfiler writeCacheProfiler;
|
||||
SolverProfiler solverProfiler;
|
||||
|
||||
bool dumpLogSwitch;
|
||||
int userId;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,288 @@
|
||||
#pragma once
|
||||
|
||||
#include "G3D/Vector3.h"
|
||||
#include "simd/simd.h"
|
||||
#include "solver/SolverContainers.h"
|
||||
#include "solver/ConstraintJacobian.h"
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
class DebugSerializer;
|
||||
|
||||
//
|
||||
// Internal representation of dynamic properties of a body (for both simulated and anchored)
|
||||
//
|
||||
class SolverBodyDynamicProperties
|
||||
{
|
||||
public:
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
Vector3 integratedLinearVelocity;
|
||||
Vector3 integratedAngularVelocity;
|
||||
Matrix3 orientation;
|
||||
Vector3 position;
|
||||
Vector3 linearVelocity;
|
||||
Vector3 angularVelocity;
|
||||
};
|
||||
|
||||
//
|
||||
// Symmetric matrix for use as inertia matrix
|
||||
//
|
||||
class SymmetricMatrix
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE Vector3 operator*( const Vector3& v ) const
|
||||
{
|
||||
Vector3 r;
|
||||
r.x = diagonals.x * v.x + offDiagonals.x * v.y + offDiagonals.y * v.z;
|
||||
r.y = offDiagonals.x * v.x + diagonals.y * v.y + offDiagonals.z * v.z;
|
||||
r.z = offDiagonals.y * v.x + offDiagonals.z * v.y + diagonals.z * v.z;
|
||||
return r;
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE SymmetricMatrix operator*( float s ) const
|
||||
{
|
||||
SymmetricMatrix r;
|
||||
r.diagonals = s * diagonals;
|
||||
r.offDiagonals = s * offDiagonals;
|
||||
return r;
|
||||
}
|
||||
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
// [ d0 a b ]
|
||||
// [ a d1 c ]
|
||||
// [ b c d2]
|
||||
Vector3Pod diagonals; // [d0, d1, d2]
|
||||
Vector3Pod offDiagonals; // [a, b, c]
|
||||
};
|
||||
|
||||
class SymmetricMatrixPOD
|
||||
{
|
||||
public:
|
||||
simd::v4f_pod diagonals;
|
||||
simd::v4f_pod offDiagonals;
|
||||
};
|
||||
|
||||
//
|
||||
// SymmetricMatrixSIMD
|
||||
//
|
||||
class SymmetricMatrixSIMD
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD( const float* _m )
|
||||
{
|
||||
diagonals = simd::load3( _m );
|
||||
offDiagonals = simd::load3( _m + 3 );
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD( const simd::v4f& _diagonal, const simd::v4f& _offDiagonal ): diagonals( _diagonal ), offDiagonals( _offDiagonal ) { }
|
||||
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD( const SymmetricMatrixPOD& _m ): diagonals( _m.diagonals ), offDiagonals( _m.offDiagonals ) { }
|
||||
|
||||
template< int row, int column >
|
||||
RBX_SIMD_INLINE simd::v4f get() const;
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f operator*( const simd::v4f& v ) const
|
||||
{
|
||||
simd::v4f t0 = diagonals * v;
|
||||
simd::v4f t1 = simd::permute<0, 2, 1, 3>( offDiagonals ) * simd::permute< 1, 2, 0, 3>( v );
|
||||
simd::v4f t2 = simd::permute<1, 0, 2, 3>( offDiagonals ) * simd::permute< 2, 0, 1, 3>( v );
|
||||
return t0 + t1 + t2;
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE void invert()
|
||||
{
|
||||
simd::v4f x00x00x02x01 = simd::shuffle< 0, 0, 1, 0 >( diagonals, offDiagonals );
|
||||
simd::v4f x11x00x01x12 = simd::shuffle< 1, 0, 0, 2 >( diagonals, offDiagonals );
|
||||
simd::v4f x22x00x12x02 = simd::shuffle< 2, 0, 2, 1 >( diagonals, offDiagonals );
|
||||
|
||||
simd::v4f x00x00x11x22 = simd::permute< 0, 0, 1, 2 >( diagonals );
|
||||
simd::v4f x12x12x02x01 = simd::permute< 2, 2, 1, 0 >( offDiagonals );
|
||||
|
||||
simd::v4f r = x00x00x02x01 * x11x00x01x12 * x22x00x12x02 - x00x00x11x22 * x12x12x02x01 * x12x12x02x01;
|
||||
simd::v4f d = simd::splat< 0 >( r ) + simd::splat< 2 >( r ) + simd::splat< 3 >( r );
|
||||
simd::v4f dInv = ( simd::splat( 1.0f ) / d );
|
||||
|
||||
simd::v4f x11x22x00 = simd::permute< 1, 2, 0, 3 >( diagonals );
|
||||
simd::v4f x22x00x11 = simd::permute< 2, 0, 1, 3 >( diagonals );
|
||||
simd::v4f x12x02x01 = simd::permute< 2, 1, 0, 3 >( offDiagonals );
|
||||
simd::v4f newDiagonals = dInv * ( x11x22x00 * x22x00x11 - x12x02x01 * x12x02x01 );
|
||||
|
||||
simd::v4f x12x01x02 = simd::permute< 2, 0, 1, 3 >( offDiagonals );
|
||||
simd::v4f x02x12x01 = simd::permute< 1, 2, 0, 3 >( offDiagonals );
|
||||
simd::v4f x01x02x12 = offDiagonals;
|
||||
simd::v4f x22x11x00 = simd::permute< 2, 1, 0, 3 >( diagonals );
|
||||
simd::v4f newOffdiagonals = dInv * ( x12x01x02 * x02x12x01 - x01x02x12 * x22x11x00 );
|
||||
|
||||
diagonals = newDiagonals;
|
||||
offDiagonals = newOffdiagonals;
|
||||
}
|
||||
|
||||
simd::v4f diagonals;
|
||||
simd::v4f offDiagonals;
|
||||
};
|
||||
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<0,0>() const { return simd::splat<0>( diagonals ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<1,1>() const { return simd::splat<1>( diagonals ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<2,2>() const { return simd::splat<2>( diagonals ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<0,1>() const { return simd::splat<0>( offDiagonals ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<0,2>() const { return simd::splat<1>( offDiagonals ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<1,2>() const { return simd::splat<2>( offDiagonals ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<1,0>() const { return get<0,1>(); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<2,0>() const { return get<0,2>(); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrixSIMD::get<2,1>() const { return get<1,2>(); }
|
||||
|
||||
static RBX_SIMD_INLINE SymmetricMatrixSIMD operator*( const simd::v4f& s, const SymmetricMatrixSIMD& m )
|
||||
{
|
||||
return SymmetricMatrixSIMD( s * m.diagonals, s* m.offDiagonals );
|
||||
}
|
||||
|
||||
//
|
||||
// SymmetricMatrix2SIMD
|
||||
//
|
||||
class SymmetricMatrix2SIMD
|
||||
{
|
||||
public:
|
||||
RBX_SIMD_INLINE SymmetricMatrix2SIMD( ) { }
|
||||
|
||||
RBX_SIMD_INLINE SymmetricMatrix2SIMD( const simd::v4f& d00, const simd::v4f& d11, const simd::v4f& d01 )
|
||||
{
|
||||
m = simd::gatherX( d00, d01, d01, d11 );
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE void load( const float* _m )
|
||||
{
|
||||
m = simd::form( _m[0], _m[2], _m[2], _m[1] );
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE void form( const simd::v4f& d00, const simd::v4f& d11, const simd::v4f& d01 )
|
||||
{
|
||||
m = simd::gatherX( d00, d01, d01, d11 );
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f operator*( const simd::v4f& v ) const
|
||||
{
|
||||
simd::v4f t0 = m * simd::permute< 0, 0, 1, 1 >( v );
|
||||
simd::v4f t1 = simd::permute< 2, 3, 0, 1 >( t0 );
|
||||
return t0 + t1;
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE void invert()
|
||||
{
|
||||
simd::v4f xt = simd::splat< 1 >( m );
|
||||
simd::v4f det = simd::splat< 0 >( m ) * simd::splat< 3 >( m ) - xt * xt;
|
||||
simd::v4f t = simd::select< 0, 1, 1, 0 >( simd::splat( 1.0f ), simd::splat( -1.0f ) ) * simd::permute< 3, 2, 1, 0 >( m );
|
||||
m = ( t / det );
|
||||
}
|
||||
|
||||
template< int row, int column >
|
||||
RBX_SIMD_INLINE simd::v4f get() const;
|
||||
|
||||
simd::v4f m;
|
||||
};
|
||||
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrix2SIMD::get<0,0>() const { return simd::splat<0>( m ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrix2SIMD::get<1,0>() const { return simd::splat<1>( m ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrix2SIMD::get<0,1>() const { return simd::splat<1>( m ); }
|
||||
template< >
|
||||
RBX_SIMD_INLINE simd::v4f SymmetricMatrix2SIMD::get<1,1>() const { return simd::splat<3>( m ); }
|
||||
|
||||
class SolverBodyMassAndInertia
|
||||
{
|
||||
public:
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD getInvInertiaVelStage() const
|
||||
{
|
||||
return inertiaSIMD;
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD getInvInertiaPosStage( float scale ) const
|
||||
{
|
||||
simd::v4f inertiaScale = simd::splat( scale ) * simd::splat( posToVelMassRatio );
|
||||
SymmetricMatrixSIMD r( inertiaSIMD );
|
||||
return inertiaScale * r;
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f getInvMassVelStage() const
|
||||
{
|
||||
return simd::splat( massInvVelStage );
|
||||
}
|
||||
|
||||
RBX_SIMD_INLINE simd::v4f getInvMassPosStage() const
|
||||
{
|
||||
return simd::splat( massInvVelStage * posToVelMassRatio );
|
||||
}
|
||||
|
||||
class VelStage;
|
||||
class PosStage;
|
||||
|
||||
template< class StageSelect >
|
||||
RBX_SIMD_INLINE simd::v4f getInvMass() const;
|
||||
|
||||
template< class StageSelect >
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD getInvInertia( float scale ) const;
|
||||
|
||||
union
|
||||
{
|
||||
struct
|
||||
{
|
||||
Vector3Pod inertiaDiagonal;
|
||||
float massInvVelStage;
|
||||
Vector3Pod inertiaOffDiagonal;
|
||||
float posToVelMassRatio;
|
||||
};
|
||||
SymmetricMatrixPOD inertiaSIMD;
|
||||
};
|
||||
};
|
||||
|
||||
template<>
|
||||
RBX_SIMD_INLINE simd::v4f SolverBodyMassAndInertia::getInvMass< SolverBodyMassAndInertia::VelStage >() const
|
||||
{
|
||||
return getInvMassVelStage();
|
||||
}
|
||||
|
||||
template<>
|
||||
RBX_SIMD_INLINE simd::v4f SolverBodyMassAndInertia::getInvMass< SolverBodyMassAndInertia::PosStage >() const
|
||||
{
|
||||
return getInvMassPosStage();
|
||||
}
|
||||
|
||||
template< >
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD SolverBodyMassAndInertia::getInvInertia< SolverBodyMassAndInertia::VelStage >( float scale ) const
|
||||
{
|
||||
return getInvInertiaVelStage();
|
||||
}
|
||||
|
||||
template< >
|
||||
RBX_SIMD_INLINE SymmetricMatrixSIMD SolverBodyMassAndInertia::getInvInertia< SolverBodyMassAndInertia::PosStage >( float scale ) const
|
||||
{
|
||||
return getInvInertiaPosStage( scale );
|
||||
}
|
||||
|
||||
//
|
||||
// Static properties of a body
|
||||
//
|
||||
class SolverBodyStaticProperties
|
||||
{
|
||||
public:
|
||||
void serialize( DebugSerializer& s ) const;
|
||||
|
||||
boost::uint64_t bodyUID;
|
||||
boost::uint32_t guid;
|
||||
bool isStatic;
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,166 @@
|
||||
#pragma once
|
||||
|
||||
#define ENABLE_SOR_CONSTRAINTS
|
||||
#define ENABLE_SOR_COLLISIONS
|
||||
#define ENABLE_LOCAL_SOR_MODULATION
|
||||
#define ENABLE_HINGE_FRICTION
|
||||
#define ENABLE_IMPULSE_CACHE_DAMPING_PER_EQUATION
|
||||
//#define ENABLE_SOLVER_PROFILER
|
||||
#define ENABLE_SOLVER_DEBUG_SERIALIZER
|
||||
//#define MIN_NORM_REPROJECT
|
||||
//#define PGS_MIN_NORM
|
||||
|
||||
//#define DISABLE_ANGULAR_CONSTRAINTS
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
class SolverConfig
|
||||
{
|
||||
public:
|
||||
enum Type
|
||||
{
|
||||
Type_Default,
|
||||
Type_InconsistencyDetector,
|
||||
Type_PositionalCorrection,
|
||||
};
|
||||
SolverConfig( Type type = Type_Default );
|
||||
|
||||
//
|
||||
// Kernel
|
||||
//
|
||||
unsigned pgsIterations;
|
||||
|
||||
//
|
||||
// Collisions
|
||||
//
|
||||
|
||||
// Minimum normal velocity for restitution to be applied
|
||||
float collisionRestitutionThreshold;
|
||||
|
||||
// Ignore penetrations that are smaller than this parameter
|
||||
float collisionPenetrationMargin;
|
||||
|
||||
// Params for variable penetration margin
|
||||
float collisionPenetrationMarginMax;
|
||||
float collisionPenetrationMarginMin;
|
||||
|
||||
// The max height of a bump that a rolling object (sphere) can have, in proportion to it's size, due to hitting an edge between two primitives.
|
||||
float collisionPenetrationMarginMaxBumpProportions;
|
||||
|
||||
// Damping of the penetration resolution
|
||||
float collisionPenetrationResolutionDamping;
|
||||
|
||||
// The velocity at which the penetration allowed will be minimum
|
||||
float collisionPenetrationVelocityForMinMargin;
|
||||
|
||||
// Threshold tangential velocity between static and dynamic friction
|
||||
float collisionFrictionStaticToDynamicThreshold;
|
||||
|
||||
// Tunning constant for static friction
|
||||
float collisionFrictionStaticScale;
|
||||
|
||||
// Tunning constant for dynamic friction
|
||||
float collisionFrictionDynamicScale;
|
||||
|
||||
//
|
||||
// Align2Axes Constraint
|
||||
//
|
||||
|
||||
// Angular friction velocity stage
|
||||
float align2AxesFrictionConstant;
|
||||
|
||||
// Angular friction position stage
|
||||
float align2AxesPositionStageFrictionConstant;
|
||||
|
||||
// Maximum angle for angular correction in degrees
|
||||
float align2AxesMaxCorrectiveAngle;
|
||||
|
||||
float align2AxesCorrectionDamping;
|
||||
|
||||
//
|
||||
// BallInSocket
|
||||
//
|
||||
|
||||
// Maximum corrective distance
|
||||
float ballInSocketMaxCorrectionByStabilization;
|
||||
|
||||
float ballInSocketCorrectionDamping;
|
||||
|
||||
//
|
||||
// SOR Modulation
|
||||
//
|
||||
|
||||
// Create a common structure for these...
|
||||
// Constraints
|
||||
struct ModulationParams
|
||||
{
|
||||
float thresholdMax;
|
||||
float thresholdMin;
|
||||
float aggressiveValue;
|
||||
float conservativeValue;
|
||||
float easingUpToAggressive;
|
||||
float easingDownToConservative;
|
||||
};
|
||||
|
||||
ModulationParams sorConstraintsModulation;
|
||||
ModulationParams sorCollisionsModulation;
|
||||
ModulationParams cacheVStageModulation;
|
||||
ModulationParams cachePStageModulation;
|
||||
|
||||
//
|
||||
// Stabilization
|
||||
//
|
||||
float stabilizationMassReductionPower;
|
||||
float stabilizationInertiaScale;
|
||||
|
||||
//
|
||||
// Cache
|
||||
//
|
||||
float velCacheDamping;
|
||||
float posCacheDamping;
|
||||
bool constraintCachingEnabled;
|
||||
|
||||
//
|
||||
// Integration
|
||||
//
|
||||
float angularDamping;
|
||||
bool updateSimBodies;
|
||||
bool integrateOnlyPositions;
|
||||
|
||||
//
|
||||
// Block PGS
|
||||
//
|
||||
bool blockPGSEnabled;
|
||||
|
||||
//
|
||||
// SOR
|
||||
//
|
||||
float velocityStageSOREnabled;
|
||||
float positionStageSOREnabled;
|
||||
|
||||
//
|
||||
// Virtual masses
|
||||
//
|
||||
bool virtualMassesEnabled;
|
||||
|
||||
//
|
||||
// Use sim islands
|
||||
//
|
||||
bool useSimIslands;
|
||||
|
||||
//
|
||||
// Conflicting constraints detector
|
||||
//
|
||||
bool inconsistentConstraintDetectorEnabled;
|
||||
unsigned inconsistentConstraintMaxIterations;
|
||||
|
||||
float inconsistentConstraintBallInSocketResidualThreshold;
|
||||
float inconsistentConstraintDeltaThreshold;
|
||||
float inconsistentConstraintAlign2AxesThreshold;
|
||||
float inconsistentConstraintCollisionThreshold;
|
||||
float inconsistentConstraintCollisionBaseThreshold;
|
||||
bool printConvergenceDiagnostics;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
#pragma once
|
||||
|
||||
#include "solver/SolverConfig.h"
|
||||
#include "boost/unordered/unordered_map.hpp"
|
||||
#include "boost/container/vector.hpp"
|
||||
|
||||
#include "rbx/DenseHash.h"
|
||||
#include "util/G3DCore.h"
|
||||
|
||||
#include <map>
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
class SimBody;
|
||||
|
||||
// If you want to have the map visible in the debugger, enable this
|
||||
// #define SOLVER_DEBUG_MAP
|
||||
|
||||
template< class K, class T >
|
||||
struct SolverUnorderedMap
|
||||
{
|
||||
#ifdef SOLVER_DEBUG_MAP
|
||||
// Use a std map in non-release for easier debugger inspection
|
||||
typedef std::map< K, T > Type;
|
||||
#else
|
||||
// This is faster than a std::map as it uses a hash table
|
||||
typedef boost::unordered::unordered_map< K, T > Type;
|
||||
#endif
|
||||
};
|
||||
|
||||
template< class K, class T >
|
||||
struct SolverOrderedMap
|
||||
{
|
||||
typedef std::map< K, T > Type;
|
||||
};
|
||||
|
||||
//typedef SolverUnorderedMap< const SimBody*, int >::Type BodyIndexation;
|
||||
typedef DenseHashMap< const SimBody*, int > BodyIndexation;
|
||||
|
||||
// It's so that we can use a Vector3 in unions
|
||||
class Vector3Pod
|
||||
{
|
||||
public:
|
||||
void operator=( const Vector3& v )
|
||||
{
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
z = v.z;
|
||||
}
|
||||
|
||||
operator Vector3() const
|
||||
{
|
||||
return Vector3(x, y, z);
|
||||
}
|
||||
|
||||
float dot( const Vector3& v ) const
|
||||
{
|
||||
return x * v.x + y * v.y + z * v.z;
|
||||
}
|
||||
|
||||
Vector3Pod& operator+=( const Vector3& v )
|
||||
{
|
||||
x += v.x;
|
||||
y += v.y;
|
||||
z += v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
float x, y, z;
|
||||
};
|
||||
|
||||
inline Vector3Pod operator*( float s, const Vector3Pod& v )
|
||||
{
|
||||
Vector3Pod r;
|
||||
r.x = s * v.x;
|
||||
r.y = s * v.y;
|
||||
r.z = s * v.z;
|
||||
return r;
|
||||
}
|
||||
|
||||
inline Vector3Pod operator+( const Vector3Pod& u, const Vector3Pod& v )
|
||||
{
|
||||
Vector3Pod r;
|
||||
r.x = u.x + v.x;
|
||||
r.y = u.y + v.y;
|
||||
r.z = u.z + v.z;
|
||||
return r;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
#pragma once
|
||||
|
||||
#include "solver/SolverConfig.h"
|
||||
#include "boost/cstdint.hpp"
|
||||
#include "rbx/ArrayDynamic.h"
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
class ConstraintJacobianPair;
|
||||
class BodyPairIndices;
|
||||
class SolverBodyStaticProperties;
|
||||
class SolverBodyMassAndInertia;
|
||||
class ConstraintVariables;
|
||||
class VirtualDisplacementArray;
|
||||
class VirtualDisplacementArray;
|
||||
class EffectiveMassPair;
|
||||
|
||||
void PGSComputeEffectiveMasses(
|
||||
EffectiveMassPair* _effectiveMassesVelStage,
|
||||
EffectiveMassPair* _effectiveMassesPosStage,
|
||||
size_t _constraintCount,
|
||||
const boost::uint8_t* _dimensions,
|
||||
const ConstraintJacobianPair* _jacobians,
|
||||
const BodyPairIndices* _pairs,
|
||||
const SolverBodyMassAndInertia* _massAndIntertia,
|
||||
const SolverConfig& _config );
|
||||
|
||||
void PGSApplyEffectiveMassMultipliers(
|
||||
EffectiveMassPair* _effectiveMassesVelStage,
|
||||
EffectiveMassPair* _effectiveMassesPosStage,
|
||||
size_t _constraintCount,
|
||||
const boost::uint8_t* _dimensions,
|
||||
const float* _multipliers,
|
||||
const BodyPairIndices* _pairs,
|
||||
const SolverConfig& _config );
|
||||
|
||||
void PGSPreconditionConstraintEquations(
|
||||
ConstraintJacobianPair* _preconditionedJacobiansVelStage,
|
||||
ConstraintJacobianPair* _preconditionedJacobiansPosStage,
|
||||
ConstraintVariables* _velocityStageVariables,
|
||||
ConstraintVariables* _positionStageVariables,
|
||||
size_t _constraintCount,
|
||||
const boost::uint8_t* _dimensions,
|
||||
const boost::uint8_t* _useBlock,
|
||||
const float* __restrict _sorVel,
|
||||
const float* __restrict _sorPos,
|
||||
const ConstraintJacobianPair* _jacobians,
|
||||
const EffectiveMassPair* _effectiveMassesVelStage,
|
||||
const EffectiveMassPair* _effectiveMassesPosStage );
|
||||
|
||||
void PGSInitVirtualDisplacements(
|
||||
VirtualDisplacementArray& _virDVel,
|
||||
VirtualDisplacementArray& _virDPos,
|
||||
const EffectiveMassPair* _effectiveMassesVelStage,
|
||||
const EffectiveMassPair* _effectiveMassesPosStage,
|
||||
size_t _constraintCount,
|
||||
const boost::uint8_t* _dimensions,
|
||||
const ConstraintVariables* __restrict _velStage,
|
||||
const ConstraintVariables* __restrict _posStage,
|
||||
const BodyPairIndices* _pairs,
|
||||
const SolverConfig& _config );
|
||||
|
||||
void PGSSolveKernel(
|
||||
ConstraintVariables* __restrict _velStage,
|
||||
ConstraintVariables* __restrict _posStage,
|
||||
VirtualDisplacementArray& _virDVel,
|
||||
VirtualDisplacementArray& _virDPos,
|
||||
size_t _constraintCount,
|
||||
size_t _collisionCount,
|
||||
const boost::uint8_t* _dimensions,
|
||||
const BodyPairIndices* _pairs,
|
||||
const ConstraintJacobianPair* _preconditionedJacobiansVelStage,
|
||||
const ConstraintJacobianPair* _preconditionedJacobiansPosStage,
|
||||
const EffectiveMassPair* _effectiveMassesVelStage,
|
||||
const EffectiveMassPair* _effectiveMassesPosStage,
|
||||
const SolverConfig& _config );
|
||||
|
||||
void PGSSolveKernelComputeErrors(
|
||||
ArrayBase< float >& _residuals,
|
||||
ArrayBase< float >& _deltaResiduals,
|
||||
ArrayBase< ConstraintVariables >& _vars,
|
||||
VirtualDisplacementArray& _virD,
|
||||
size_t _constraintCount,
|
||||
size_t _collisionCount,
|
||||
size_t _bodyCount,
|
||||
const boost::uint8_t* _dimensions,
|
||||
const BodyPairIndices* _pairs,
|
||||
const ConstraintJacobianPair* _jacobians,
|
||||
const ConstraintJacobianPair* _preconditionedJacobians,
|
||||
const EffectiveMassPair* _effectiveMasses,
|
||||
const SolverConfig& _config );
|
||||
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
#pragma once
|
||||
|
||||
#include "rbx/rbxTime.h"
|
||||
#include "RbxAssert.h"
|
||||
#include "util/standardout.h"
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
//
|
||||
// Prints average timing after the given number of samples were taken
|
||||
//
|
||||
class SolverProfiler
|
||||
{
|
||||
public:
|
||||
SolverProfiler( int _samples, const char* _format ): maxSamples( _samples ), format( _format )
|
||||
{
|
||||
accumulator = Time::Interval::zero();
|
||||
currentSamples = 0;
|
||||
}
|
||||
|
||||
void start()
|
||||
{
|
||||
#ifdef ENABLE_SOLVER_PROFILER
|
||||
startTime = Time::now( Time::Precise );
|
||||
#endif
|
||||
}
|
||||
|
||||
void end()
|
||||
{
|
||||
#ifdef ENABLE_SOLVER_PROFILER
|
||||
Time::Interval total = Time::now( Time::Precise ) - startTime;
|
||||
accumulator += total;
|
||||
currentSamples++;
|
||||
#endif
|
||||
}
|
||||
|
||||
void printStats()
|
||||
{
|
||||
#ifdef ENABLE_SOLVER_PROFILER
|
||||
static bool enable = true;
|
||||
if( currentSamples >= maxSamples && enable )
|
||||
{
|
||||
currentSamples = 0;
|
||||
StandardOut::singleton()->printf( MESSAGE_OUTPUT, format, accumulator.msec() / maxSamples );
|
||||
accumulator = Time::Interval::zero();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
private:
|
||||
RBX::Time startTime;
|
||||
Time::Interval accumulator;
|
||||
const char* format;
|
||||
int maxSamples;
|
||||
int currentSamples;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,189 @@
|
||||
#pragma once
|
||||
|
||||
#include "solver/SolverConfig.h"
|
||||
#include "solver/SolverContainers.h"
|
||||
#include "solver/DebugSerializer.h"
|
||||
|
||||
#include "util/standardout.h"
|
||||
|
||||
#include "boost/filesystem.hpp"
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
|
||||
class SolverSerializer
|
||||
{
|
||||
public:
|
||||
SolverSerializer(): enabled( false ), fileOpened( false ) { }
|
||||
|
||||
void update( bool _enabled, int userId, boost::uint64_t debugTime )
|
||||
{
|
||||
#ifdef ENABLE_SOLVER_DEBUG_SERIALIZER
|
||||
bool switchState = ( enabled != _enabled );
|
||||
bool close = false;
|
||||
if( switchState )
|
||||
{
|
||||
if( !enabled )
|
||||
{
|
||||
enabled = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
enabled = false;
|
||||
close = true;
|
||||
}
|
||||
}
|
||||
else if( !enabled )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
static size_t bufferSize = 10 * 1024 * 1024;
|
||||
|
||||
if( !fileOpened )
|
||||
{
|
||||
debugSerializer.data.reserve( bufferSize + 1024 * 1024 );
|
||||
boost::filesystem::path path = boost::filesystem::temp_directory_path();
|
||||
path /= "ROBLOX";
|
||||
path /= "SolverLog_Client";
|
||||
path += boost::lexical_cast<std::string>( userId );
|
||||
path += ".bin";
|
||||
myFile.open (path.c_str(), std::ios::out | std::ios::binary);
|
||||
fileOpened = true;
|
||||
debugSerializer & userId;
|
||||
}
|
||||
|
||||
if( close || debugSerializer.data.size() > bufferSize )
|
||||
{
|
||||
myFile.write( debugSerializer.data.data(), debugSerializer.data.size() );
|
||||
debugSerializer.data.clear();
|
||||
}
|
||||
|
||||
if( close && fileOpened )
|
||||
{
|
||||
close = false;
|
||||
enabled = false;
|
||||
myFile.close();
|
||||
fileOpened = false;
|
||||
}
|
||||
|
||||
static boost::uint64_t frame = 0;
|
||||
if( enabled )
|
||||
{
|
||||
debugSerializer & frame;
|
||||
debugSerializer & debugTime;
|
||||
frame++;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void serializeConstraints( const ArrayBase< Constraint* >& connectors )
|
||||
{
|
||||
if( enabled )
|
||||
{
|
||||
debugSerializer.tag("Connectors");
|
||||
debugSerializer & (boost::uint32_t)connectors.size();
|
||||
for( const auto* c : connectors )
|
||||
{
|
||||
debugSerializer & (boost::uint8_t)c->getType() & c;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void serializeForces( const ArrayBase< SimBody* >& simBodies, boost::uint32_t total )
|
||||
{
|
||||
if( enabled )
|
||||
{
|
||||
debugSerializer.tag("Forces");
|
||||
debugSerializer & total;
|
||||
size_t i = 0;
|
||||
for( i = 0; i < simBodies.size(); i++ )
|
||||
{
|
||||
debugSerializer & simBodies[ i ]->getForce();
|
||||
debugSerializer & simBodies[ i ]->getTorque();
|
||||
debugSerializer & simBodies[ i ]->getImpulse();
|
||||
debugSerializer & simBodies[ i ]->getRotationallmpulse();
|
||||
}
|
||||
Vector3 zero = Vector3::zero();
|
||||
for( ; i < total; i++ )
|
||||
{
|
||||
debugSerializer & zero & zero & zero & zero;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void serializeComputedImpulse( const ArrayBase< ConstraintVariables >& velocityStage, const ArrayBase< ConstraintVariables >& positionStage )
|
||||
{
|
||||
if(enabled)
|
||||
{
|
||||
ArrayDynamic< float > impulses;
|
||||
impulses.reserve(velocityStage.size());
|
||||
for( const auto& v : velocityStage )
|
||||
{
|
||||
impulses.push_back(v.impulse);
|
||||
}
|
||||
debugSerializer & impulses;
|
||||
impulses.clear();
|
||||
impulses.reserve(velocityStage.size());
|
||||
for( const auto& v : positionStage )
|
||||
{
|
||||
impulses.push_back(v.impulse);
|
||||
}
|
||||
debugSerializer & impulses;
|
||||
}
|
||||
}
|
||||
|
||||
template< class Cache >
|
||||
void serializeBodyCache( const Cache& bodyCache )
|
||||
{
|
||||
if( enabled )
|
||||
{
|
||||
debugSerializer & boost::uint32_t( bodyCache.size() );
|
||||
for( const auto& it : bodyCache )
|
||||
{
|
||||
debugSerializer & (boost::uint32_t)it.second.simBodyDebug->getBody()->getGuidIndex();
|
||||
debugSerializer & it.second;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template< class T >
|
||||
SolverSerializer& operator&( const T& t )
|
||||
{
|
||||
#ifdef ENABLE_SOLVER_DEBUG_SERIALIZER
|
||||
if( enabled )
|
||||
{
|
||||
debugSerializer & t;
|
||||
}
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
template< class T >
|
||||
SolverSerializer& operator&( const ArrayDynamic< T >& t )
|
||||
{
|
||||
#ifdef ENABLE_SOLVER_DEBUG_SERIALIZER
|
||||
if( enabled )
|
||||
{
|
||||
debugSerializer & static_cast< const ArrayBase< T >& >( t );
|
||||
}
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
SolverSerializer& tag( const char* name )
|
||||
{
|
||||
if( enabled )
|
||||
{
|
||||
debugSerializer.tag(name);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
std::ofstream myFile;
|
||||
bool fileOpened;
|
||||
bool enabled;
|
||||
DebugSerializer debugSerializer;
|
||||
};
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user