public btHinge2Constraint(btRigidBody rbA, btRigidBody rbB, btVector3 anchor, btVector3 axis1, btVector3 axis2) : this(BulletPINVOKE.new_btHinge2Constraint(btRigidBody.getCPtr(rbA), btRigidBody.getCPtr(rbB), btVector3.getCPtr(anchor), btVector3.getCPtr(axis1), btVector3.getCPtr(axis2)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btGeneric6DofSpringConstraint(btRigidBody rbB, btTransform frameInB, bool useLinearReferenceFrameB) : this(BulletPINVOKE.new_btGeneric6DofSpringConstraint__SWIG_1(btRigidBody.getCPtr(rbB), btTransform.getCPtr(frameInB), useLinearReferenceFrameB), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btGearConstraint(btRigidBody rbA, btRigidBody rbB, btVector3 axisInA, btVector3 axisInB) : this(BulletPINVOKE.new_btGearConstraint__SWIG_1(btRigidBody.getCPtr(rbA), btRigidBody.getCPtr(rbB), btVector3.getCPtr(axisInA), btVector3.getCPtr(axisInB)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btRaycastVehicle(btRaycastVehicle.btVehicleTuning tuning, btRigidBody chassis, btVehicleRaycaster raycaster) : this(BulletPINVOKE.new_btRaycastVehicle(btRaycastVehicle.btVehicleTuning.getCPtr(tuning), btRigidBody.getCPtr(chassis), btVehicleRaycaster.getCPtr(raycaster)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btRigidBody getRigidBody()
{
global::System.IntPtr cPtr = BulletPINVOKE.btRaycastVehicle_getRigidBody__SWIG_0(swigCPtr);
btRigidBody ret = (cPtr == global::System.IntPtr.Zero) ? null : new btRigidBody(cPtr, false);
return(ret);
}
public static btRigidBody upcast(btCollisionObject colObj)
{
global::System.IntPtr cPtr = BulletPINVOKE.btRigidBody_upcast__SWIG_0(btCollisionObject.getCPtr(colObj));
btRigidBody ret = (cPtr == global::System.IntPtr.Zero) ? null : new btRigidBody(cPtr, false);
return(ret);
}
public btFixedConstraint(btRigidBody rbA, btRigidBody rbB, btTransform frameInA, btTransform frameInB) : this(BulletPINVOKE.new_btFixedConstraint(btRigidBody.getCPtr(rbA), btRigidBody.getCPtr(rbB), btTransform.getCPtr(frameInA), btTransform.getCPtr(frameInB)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btPoint2PointConstraint(btRigidBody rbA, btVector3 pivotInA) : this(BulletPINVOKE.new_btPoint2PointConstraint__SWIG_1(btRigidBody.getCPtr(rbA), btVector3.getCPtr(pivotInA)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btSliderConstraint(btRigidBody rbA, btRigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA) : this(BulletPINVOKE.new_btSliderConstraint__SWIG_0(btRigidBody.getCPtr(rbA), btRigidBody.getCPtr(rbB), btTransform.getCPtr(frameInA), btTransform.getCPtr(frameInB), useLinearReferenceFrameA), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
internal void Dispose()
{
if (Body.isInWorld())
{
m_parent_scene.removeFromWorld(Body);
}
if (m_aMotor.Handle != IntPtr.Zero)
{
m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
}
m_aMotor.Dispose(); m_aMotor = null;
ClosestCastResult.Dispose(); ClosestCastResult = null;
Body.Dispose(); Body = null;
Shell.Dispose(); Shell = null;
tempQuat1.Dispose();
tempTrans1.Dispose();
tempVector1.Dispose();
tempVector2.Dispose();
tempVector3.Dispose();
tempVector4.Dispose();
tempVector5RayCast.Dispose();
tempVector6RayCast.Dispose();
}
public btConeTwistConstraint(btRigidBody rbA, btTransform rbAFrame) : this(BulletPINVOKE.new_btConeTwistConstraint__SWIG_1(btRigidBody.getCPtr(rbA), btTransform.getCPtr(rbAFrame)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btHingeConstraint(btRigidBody rbA, btTransform rbAFrame, bool useReferenceFrameA) : this(BulletPINVOKE.new_btHingeConstraint__SWIG_6(btRigidBody.getCPtr(rbA), btTransform.getCPtr(rbAFrame), useReferenceFrameA), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btHingeConstraint(btRigidBody rbA, btVector3 pivotInA, btVector3 axisInA, bool useReferenceFrameA) : this(BulletPINVOKE.new_btHingeConstraint__SWIG_2(btRigidBody.getCPtr(rbA), btVector3.getCPtr(pivotInA), btVector3.getCPtr(axisInA), useReferenceFrameA), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void updateWheel(btRigidBody chassis, btWheelInfo.RaycastInfo raycastInfo)
{
BulletPINVOKE.btWheelInfo_updateWheel(swigCPtr, btRigidBody.getCPtr(chassis), btWheelInfo.RaycastInfo.getCPtr(raycastInfo));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public float solveLinearAxis(float timeStep, float jacDiagABInv, btRigidBody body1, btVector3 pointInA, btRigidBody body2, btVector3 pointInB, int limit_index, btVector3 axis_normal_on_a, btVector3 anchorPos)
{
float ret = BulletPINVOKE.btTranslationalLimitMotor_solveLinearAxis(swigCPtr, timeStep, jacDiagABInv, btRigidBody.getCPtr(body1), btVector3.getCPtr(pointInA), btRigidBody.getCPtr(body2), btVector3.getCPtr(pointInB), limit_index, btVector3.getCPtr(axis_normal_on_a), btVector3.getCPtr(anchorPos));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
internal void removeFromWorld(BulletDotNETCharacter chr, btRigidBody body)
{
lock (m_characters)
{
if (m_characters.Contains(chr))
{
m_world.removeRigidBody(body);
m_characters.Remove(chr);
}
}
}
internal void removeFromWorld(BulletDotNETPrim prm, btRigidBody body)
{
lock (m_prims)
{
if (m_prims.Contains(prm))
{
m_world.removeRigidBody(body);
}
remActivePrim(prm);
m_prims.Remove(prm);
}
}
protected override void OnCreate(Bundle bundle)
{
base.OnCreate(bundle);
// Set our view from the "main" layout resource
SetContentView(Resource.Layout.Main);
btBroadphaseInterface broadphase = new btDbvtBroadphase();
btDefaultCollisionConfiguration collisionConfiguration = new btDefaultCollisionConfiguration();
btCollisionDispatcher dispatcher = new btCollisionDispatcher(collisionConfiguration);
btSequentialImpulseConstraintSolver solver = new btSequentialImpulseConstraintSolver();
btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
dynamicsWorld.setGravity(new btVector3(0, -10, 0));
btCollisionShape groundShape = new btStaticPlaneShape(new btVector3(0, 1, 0), 1);
btDefaultMotionState groundMotionState = new btDefaultMotionState(new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, -1, 0)));
btRigidBody.btRigidBodyConstructionInfo groundRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo(0, groundMotionState, groundShape, new btVector3(0, 0, 0));
btRigidBody groundRigidBody = new btRigidBody(groundRigidBodyCI);
dynamicsWorld.addRigidBody(groundRigidBody);
btDefaultMotionState fallMotionState = new btDefaultMotionState(new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 50, 0)));
btVector3 fallInertia = new btVector3(0, 0, 0);
btCollisionShape fallShape = new btSphereShape(1);
fallShape.calculateLocalInertia(1, fallInertia);
btRigidBody.btRigidBodyConstructionInfo fallRigidBodyCI = new btRigidBody.btRigidBodyConstructionInfo(1, fallMotionState, fallShape, fallInertia);
btRigidBody fallRigidBody = new btRigidBody(fallRigidBodyCI);
dynamicsWorld.addRigidBody(fallRigidBody);
for (int i = 0; i < 300; i++)
{
dynamicsWorld.stepSimulation(1 / 60.0f, 10);
btTransform trans = new btTransform();
fallRigidBody.getMotionState().getWorldTransform(trans);
Console.WriteLine("sphere height: " + trans.getOrigin().getY());
}
}
// // solving 2x2 lcp problem (inequality, direct solution ) // void resolveBilateralPairConstraint( btRigidBody body0, btRigidBody body1, btMatrix3x3& world2A, btMatrix3x3& world2B, ref btVector3 invInertiaADiag, double invMassA, ref btVector3 linvelA, ref btVector3 angvelA, ref btVector3 rel_posA1, ref btVector3 invInertiaBDiag, double invMassB, ref btVector3 linvelB, ref btVector3 angvelB, ref btVector3 rel_posA2, double depthA, ref btVector3 normalA, ref btVector3 rel_posB1, ref btVector3 rel_posB2, double depthB, ref btVector3 normalB, double imp0, double imp1 );
internal void Dispose()
{
m_parent_scene.RemoveAvatarFromList(this);
if (Body != null)
{
if (Body.isInWorld())
{
m_parent_scene.removeFromWorld(this, Body);
m_parent_scene.RemoveCollisionObject(Body);
}
Body.Dispose();
Body = null;
}
if (m_aMotor != null)
{
if (m_aMotor.Handle != IntPtr.Zero)
{
m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
}
m_aMotor.Dispose(); m_aMotor = null;
}
if (ClosestCastResult != null)
{
ClosestCastResult.Dispose(); ClosestCastResult = null;
}
if (Shell != null)
{
Shell.Dispose(); Shell = null;
}
tempQuat1.Dispose();
tempTrans1.Dispose();
tempVector1.Dispose();
tempVector2.Dispose();
tempVector3.Dispose();
tempVector4.Dispose();
tempVector5RayCast.Dispose();
tempVector6RayCast.Dispose();
}
btPoint2PointConstraint( btRigidBody rbA, ref btVector3 pivotInA )
: base( btObjectTypes.POINT2POINT_CONSTRAINT_TYPE, rbA )
{
m_pivotInA = ( pivotInA );
rbA.m_worldTransform.Apply( ref pivotInA, out m_pivotInB );
m_flags = ( 0 );
m_useSolveConstraintObsolete = ( false );
}
btContactConstraint(btPersistentManifold* contactManifold,btRigidBody rbA,btRigidBody rbB);
internal btTypedConstraint( btObjectTypes type, btRigidBody rbA, btRigidBody rbB )
{
base.Initialize( type );
m_userConstraintType = ( -1 );
m_userConstraintPtr = ( (object)-1 );
m_breakingImpulseThreshold = ( btScalar.SIMD_INFINITY );
m_isEnabled = ( true );
m_needsFeedback = ( false );
m_overrideNumSolverIterations = ( -1 );
m_rbA = ( rbA );
m_rbB = ( rbB );
m_appliedImpulse = ( (double)( 0.0 ) );
m_dbgDrawSize = ( DEFAULT_DEBUGDRAW_SIZE );
m_jointFeedback = ( null );
}
public btRigidBody getRigidBodyB()
{
btRigidBody ret = new btRigidBody(BulletPINVOKE.btTypedConstraint_getRigidBodyB__SWIG_0(swigCPtr), false);
return(ret);
}
internal void removeFromWorld(btRigidBody body)
{
m_world.removeRigidBody(body);
}
internal btGeneric6DofConstraint( btRigidBody rbB, ref btTransform frameInB, bool useLinearReferenceFrameB )
: base( btObjectTypes.D6_CONSTRAINT_TYPE, getFixedBody(), rbB )
{
m_frameInB = ( frameInB );
m_useLinearReferenceFrameA = ( useLinearReferenceFrameB );
m_useOffsetForConstraintFrame = ( D6_USE_FRAME_OFFSET );
m_flags = ( 0 );
m_useSolveConstraintObsolete = ( false );
///not providing rigidbody A means implicitly using worldspace for body A
rbB.m_worldTransform.Apply( ref m_frameInB, out m_frameInA );
calculateTransforms();
}
btFixedConstraint(btRigidBody rbA,btRigidBody rbB, ref btTransform frameInA,ref btTransform frameInB);
public btHingeConstraint( btRigidBody rbA, btRigidBody rbB, ref btVector3 pivotInA, ref btVector3 pivotInB,
ref btVector3 axisInA, ref btVector3 axisInB, bool useReferenceFrameA = false )
: base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA, rbB )
{
Init();
#if _BT_USE_CENTER_LIMIT_
m_limit = new btAngularLimit();
#endif
m_useReferenceFrameA = ( useReferenceFrameA );
m_rbAFrame.m_origin = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1; rbA.m_worldTransform.m_basis.getColumn( 0, out rbAxisA1 );
btVector3 rbAxisA2;
double projection = axisInA.dot( rbAxisA1 );
if( projection >= 1.0f - btScalar.SIMD_EPSILON )
{
btVector3 tmp;
rbA.m_worldTransform.m_basis.getColumn( 2, out tmp );
tmp.Invert( out rbAxisA1 );
rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
}
else if( projection <= -1.0f + btScalar.SIMD_EPSILON )
{
rbA.m_worldTransform.m_basis.getColumn( 2, out rbAxisA1 );
rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
}
else
{
axisInA.cross( ref rbAxisA1, out rbAxisA2 );
rbAxisA2.cross( ref axisInA, out rbAxisA1 );
}
btMatrix3x3.setValue( out m_rbAFrame.m_basis, rbAxisA1.x, rbAxisA2.x, axisInA.x,
rbAxisA1.y, rbAxisA2.y, axisInA.y,
rbAxisA1.z, rbAxisA2.z, axisInA.z );
btQuaternion rotationArc; btQuaternion.shortestArcQuat( ref axisInA, ref axisInB, out rotationArc );
btVector3 rbAxisB1; btQuaternion.quatRotate( ref rotationArc, ref rbAxisA1, out rbAxisB1 );
btVector3 rbAxisB2; axisInB.cross( ref rbAxisB1, out rbAxisB2 );
m_rbBFrame.m_origin = pivotInB;
m_rbBFrame.m_basis.setValue( ref rbAxisB1, ref rbAxisB2, ref axisInB );
btMatrix3x3.setValue( out m_rbBFrame.m_basis, ref rbAxisB1, ref rbAxisB2, ref axisInB );
m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 );
}
public btHingeAccumulatedAngleConstraint( btRigidBody rbA, ref btTransform rbAFrame, bool useReferenceFrameA = false )
: base( rbA, ref rbAFrame, useReferenceFrameA )
{
m_accumulatedAngle = getHingeAngle();
}
public btHingeAccumulatedAngleConstraint( btRigidBody rbA, ref btVector3 pivotInA, ref btVector3 axisInA, bool useReferenceFrameA = false )
: base( rbA, ref pivotInA, ref axisInA, useReferenceFrameA )
{
m_accumulatedAngle = getHingeAngle();
}
internal btSliderConstraint( btRigidBody rbB, ref btTransform frameInB, bool useLinearReferenceFrameA )
: base( btObjectTypes.SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB )
{
//m_useSolveConstraintObsolete = (false),
m_frameInB = ( frameInB );
m_useLinearReferenceFrameA = ( useLinearReferenceFrameA );
///not providing rigidbody A means implicitly using worldspace for body A
rbB.m_worldTransform.Apply( ref m_frameInB, out m_frameInA );
// m_frameInA.m_origin = m_rbA.m_worldTransform(m_frameInA.m_origin);
initParams();
}
btGearConstraint(btRigidBody rbA, btRigidBody rbB, ref btVector3 axisInA,ref btVector3 axisInB, double ratio=1);
public override void addRigidBody(btRigidBody body, short group, short mask)
{
BulletPINVOKE.btDiscreteDynamicsWorld_addRigidBody__SWIG_1(swigCPtr, btRigidBody.getCPtr(body), group, mask);
}
/*
void setAxis( ref btVector3 axis1, ref btVector3 axis2 );
void setBounce( int index, double bounce );
void enableMotor( int index, bool onOff );
void setServo( int index, bool onOff ); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
void setTargetVelocity( int index, double velocity );
void setServoTarget( int index, double target );
void setMaxMotorForce( int index, double force );
void enableSpring( int index, bool onOff );
void setStiffness( int index, double stiffness, bool limitIfNeeded = true ); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
void setDamping( int index, double damping, bool limitIfNeeded = true ); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
void setEquilibriumPoint( int index ); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint( int index, double val );
*/
//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
//If no axis is provided, it uses the default axis for this constraint.
internal btGeneric6DofSpring2Constraint( btRigidBody rbA, btRigidBody rbB, ref btTransform frameInA, ref btTransform frameInB, RotateOrder rotOrder )
: base( btObjectTypes.D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB )
{
m_frameInA = ( frameInA );
m_frameInB = ( frameInB );
m_rotateOrder = ( rotOrder );
m_flags = ( 0 );
calculateTransforms();
}
public void synchronizeSingleMotionState(btRigidBody body)
{
BulletPINVOKE.btDiscreteDynamicsWorld_synchronizeSingleMotionState(swigCPtr, btRigidBody.getCPtr(body));
}
internal btSliderConstraint( btRigidBody rbA, btRigidBody rbB, ref btTransform frameInA, ref btTransform frameInB, bool useLinearReferenceFrameA )
: base( btObjectTypes.SLIDER_CONSTRAINT_TYPE, rbA, rbB )
{
//m_useSolveConstraintObsolete = ( false ),
m_frameInA = ( frameInA );
m_frameInB = ( frameInB );
m_useLinearReferenceFrameA = ( useLinearReferenceFrameA );
initParams();
}
internal btGeneric6DofConstraint( btRigidBody rbA, btRigidBody rbB, ref btTransform frameInA, ref btTransform frameInB, bool useLinearReferenceFrameA )
: base( btObjectTypes.D6_CONSTRAINT_TYPE, rbA, rbB )
{
m_frameInA = ( frameInA );
m_frameInB = ( frameInB );
m_useLinearReferenceFrameA = ( useLinearReferenceFrameA );
m_useOffsetForConstraintFrame = ( D6_USE_FRAME_OFFSET );
m_flags = ( 0 );
m_useSolveConstraintObsolete = ( D6_USE_OBSOLETE_METHOD );
calculateTransforms();
}
public btHingeConstraint( btRigidBody rbA, ref btTransform rbAFrame, bool useReferenceFrameA = false )
: base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA )
{
Init();
m_rbAFrame = ( rbAFrame );
m_rbBFrame = ( rbAFrame );
#if _BT_USE_CENTER_LIMIT_
m_limit = new btAngularLimit();
#endif
//m_useSolveConstraintObsolete = ( false/*HINGE_USE_OBSOLETE_SOLVER*/ );
//m_useOffsetForConstraintFrame = ( true/*HINGE_USE_FRAME_OFFSET*/ );
m_useReferenceFrameA = ( useReferenceFrameA );
///not providing rigidbody B means implicitly using worldspace for body B
m_rbA.m_worldTransform.Apply( ref m_rbAFrame.m_origin, out m_rbBFrame.m_origin );
//m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()( m_rbAFrame.getOrigin() );
m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 );
}
internal void AddRigidBody(btRigidBody Body)
{
m_world.addRigidBody(Body);
}
void updateWheel( btRigidBody chassis, RaycastInfo& raycastInfo);
public static btRigidBody getFixedBody()
{
btRigidBody ret = new btRigidBody(BulletPINVOKE.btTypedConstraint_getFixedBody(), false);
return(ret);
}
btSliderConstraint( btRigidBody rbB, ref btTransform frameInB, bool useLinearReferenceFrameA );
btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody bodyB, ref btVector3 pivotInA, ref btVector3 pivotInB);
public virtual void addRigidBody(btRigidBody body, short group, short mask)
{
BulletPINVOKE.btDynamicsWorld_addRigidBody__SWIG_1(swigCPtr, btRigidBody.getCPtr(body), group, mask);
}
public btConeTwistConstraint( btRigidBody rbA, ref btTransform rbAFrame )
: base( btObjectTypes.CONETWIST_CONSTRAINT_TYPE, rbA )
{
m_rbAFrame = ( rbAFrame );
m_angularOnly = ( false );
m_useSolveConstraintObsolete = ( CONETWIST_USE_OBSOLETE_SOLVER );
m_rbBFrame = m_rbAFrame;
m_rbBFrame.setOrigin( ref btVector3.Zero );
init();
}
public virtual void removeRigidBody(btRigidBody body)
{
BulletPINVOKE.btDynamicsWorld_removeRigidBody(swigCPtr, btRigidBody.getCPtr(body));
}
/*
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam( int num, double value, int axis = -1 );
///return the local value of parameter
virtual double getParam( int num, int axis = -1 );
*/
btPoint2PointConstraint( btRigidBody rbA, btRigidBody rbB, ref btVector3 pivotInA, ref btVector3 pivotInB )
: base( btObjectTypes.POINT2POINT_CONSTRAINT_TYPE, rbA, rbB )
{
m_pivotInA = ( pivotInA );
m_pivotInB = ( pivotInB );
m_flags = ( 0 );
m_useSolveConstraintObsolete = ( false );
}
public override void SetTerrain(float[] heightMap)
{
if (m_terrainShape != null)
{
DeleteTerrain();
}
float hfmax = -9000;
float hfmin = 90000;
for (int i = 0; i < heightMap.Length; i++)
{
if (Single.IsNaN(heightMap[i]) || Single.IsInfinity(heightMap[i]))
{
heightMap[i] = 0f;
}
hfmin = (heightMap[i] < hfmin) ? heightMap[i] : hfmin;
hfmax = (heightMap[i] > hfmax) ? heightMap[i] : hfmax;
}
// store this for later reference.
// Note, we're storing it after we check it for anomolies above
_origheightmap = heightMap;
hfmin = 0;
hfmax = 256;
m_terrainShape = new btHeightfieldTerrainShape((int)Constants.RegionSize, (int)Constants.RegionSize, heightMap,
1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false);
float AabbCenterX = Constants.RegionSize / 2f;
float AabbCenterY = Constants.RegionSize / 2f;
float AabbCenterZ = 0;
float temphfmin, temphfmax;
temphfmin = hfmin;
temphfmax = hfmax;
if (temphfmin < 0)
{
temphfmax = 0 - temphfmin;
temphfmin = 0 - temphfmin;
}
else if (temphfmin > 0)
{
temphfmax = temphfmax + (0 - temphfmin);
//temphfmin = temphfmin + (0 - temphfmin);
}
AabbCenterZ = temphfmax / 2f;
if (m_terrainPosition == null)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
else
{
try
{
m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
}
catch (ObjectDisposedException)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
}
if (m_terrainMotionState != null)
{
m_terrainMotionState.Dispose();
m_terrainMotionState = null;
}
m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
TerrainBody.setUserPointer((IntPtr)0);
m_world.addRigidBody(TerrainBody);
}
public override void addRigidBody(btRigidBody body)
{
BulletPINVOKE.btDiscreteDynamicsWorld_addRigidBody__SWIG_0(swigCPtr, btRigidBody.getCPtr(body));
}
//! apply the correction impulses for two bodies
//double solveAngularLimits(double timeStep,ref btVector3 axis, double jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
internal double solveAngularLimits(
double timeStep, ref btVector3 axis, double jacDiagABInv,
btRigidBody body0, btRigidBody body1 )
{
if( needApplyTorques() == false ) return 0.0f;
double target_velocity = m_targetVelocity;
double maxMotorForce = m_maxMotorForce;
//current error correction
if( m_currentLimit != 0 )
{
target_velocity = -m_stopERP * m_currentLimitError / ( timeStep );
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
btVector3 angVelA = body0.getAngularVelocity();
btVector3 angVelB = body1.getAngularVelocity();
btVector3 vel_diff;
vel_diff = angVelA - angVelB;
double rel_vel = axis.dot( vel_diff );
// correction velocity
double motor_relvel = m_limitSoftness * ( target_velocity - m_damping * rel_vel );
if( motor_relvel < btScalar.SIMD_EPSILON && motor_relvel > -btScalar.SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
double unclippedMotorImpulse = ( 1 + m_bounce ) * motor_relvel * jacDiagABInv;
// clip correction impulse
double clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if( unclippedMotorImpulse > 0.0f )
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
}
// sort with accumulated impulses
double lo = (double)( -btScalar.BT_LARGE_FLOAT );
double hi = (double)( btScalar.BT_LARGE_FLOAT );
double oldaccumImpulse = m_accumulatedImpulse;
double sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
btVector3 motorImp; axis.Mult( clippedMotorImpulse, out motorImp );
body0.applyTorqueImpulse( ref motorImp );
btVector3 tmp;
motorImp.Invert( out tmp );
body1.applyTorqueImpulse( ref tmp );
return clippedMotorImpulse;
}
public override void removeRigidBody(btRigidBody body)
{
BulletPINVOKE.btDiscreteDynamicsWorld_removeRigidBody(swigCPtr, btRigidBody.getCPtr(body));
}
internal double solveLinearAxis(
double timeStep,
double jacDiagABInv,
btRigidBody body1, btVector3 pointInA,
btRigidBody body2, btVector3 pointInB,
int limit_index,
btVector3 axis_normal_on_a,
btVector3 anchorPos )
{
///find relative velocity
// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = anchorPos - body1.m_worldTransform.m_origin;
btVector3 rel_pos2 = anchorPos - body2.m_worldTransform.m_origin;
btVector3 vel1 = body1.getVelocityInLocalPoint( ref rel_pos1 );
btVector3 vel2 = body2.getVelocityInLocalPoint( ref rel_pos2 );
btVector3 vel = vel1 - vel2;
double rel_vel = axis_normal_on_a.dot( vel );
/// apply displacement correction
//positional error (zeroth order error)
double depth = -( pointInA - pointInB ).dot( axis_normal_on_a );
double lo = (double)( -btScalar.BT_LARGE_FLOAT );
double hi = (double)( btScalar.BT_LARGE_FLOAT );
double minLimit = m_lowerLimit[limit_index];
double maxLimit = m_upperLimit[limit_index];
//handle the limits
if( minLimit < maxLimit )
{
{
if( depth > maxLimit )
{
depth -= maxLimit;
lo = btScalar.BT_ZERO;
}
else
{
if( depth < minLimit )
{
depth -= minLimit;
hi = btScalar.BT_ZERO;
}
else
{
return 0.0f;
}
}
}
}
double normalImpulse = m_limitSoftness * ( m_restitution * depth / timeStep - m_damping * rel_vel ) * jacDiagABInv;
double oldNormalImpulse = m_accumulatedImpulse[limit_index];
double sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1.applyImpulse( ref impulse_vector, ref rel_pos1 );
btVector3 tmp;
impulse_vector.Invert( out tmp );
body2.applyImpulse( ref tmp, ref rel_pos2 );
return normalImpulse;
}
public btHingeConstraint( btRigidBody rbA, ref btVector3 pivotInA, ref btVector3 axisInA, bool useReferenceFrameA = false )
: base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA )
{
Init();
#if _BT_USE_CENTER_LIMIT_
m_limit = new btAngularLimit();
#endif
m_useReferenceFrameA = ( useReferenceFrameA );
// since no frame is given; assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace
btVector3 rbAxisA1, rbAxisA2;
btVector3.btPlaneSpace1( ref axisInA, out rbAxisA1, out rbAxisA2 );
m_rbAFrame.m_origin = pivotInA;
m_rbAFrame.m_basis.setValue( rbAxisA1.x, rbAxisA2.x, axisInA.x,
rbAxisA1.y, rbAxisA2.y, axisInA.y,
rbAxisA1.z, rbAxisA2.z, axisInA.z );
btVector3 axisInB; rbA.m_worldTransform.m_basis.Apply( ref axisInA, out axisInB );
btQuaternion rotationArc; btQuaternion.shortestArcQuat( ref axisInA, ref axisInB, out rotationArc );
btVector3 rbAxisB1; btQuaternion.quatRotate( ref rotationArc, ref rbAxisA1, out rbAxisB1 );
btVector3 rbAxisB2; axisInB.cross( ref rbAxisB1, out rbAxisB2 );
//btVector3 tmp;
rbA.m_worldTransform.Apply( ref pivotInA, out m_rbBFrame.m_origin );
//m_rbBFrame.m_origin = rbA.getCenterOfMassTransform()( pivotInA );
m_rbBFrame.m_basis.setValue( rbAxisB1.x, rbAxisB2.x, axisInB.x,
rbAxisB1.y, rbAxisB2.y, axisInB.y,
rbAxisB1.z, rbAxisB2.z, axisInB.z );
m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 );
}
protected void initSolverBody( btSolverBody solverBody, btRigidBody rb, double timeStep )
{
//btRigidBody rb = collisionObject != null ? btRigidBody.upcast( collisionObject ) : null;
solverBody.m_deltaLinearVelocity = btVector3.Zero;
solverBody.m_deltaAngularVelocity = btVector3.Zero;
solverBody.m_pushVelocity = btVector3.Zero;
solverBody.m_turnVelocity = btVector3.Zero;
solverBody.modified = false;
solverBody.pushed = false;
if( rb != null )
{
solverBody.m_worldTransform = rb.m_worldTransform;
btVector3 tmp = new btVector3( rb.getInvMass() );
btVector3 tmp2;
btVector3 tmp3;
rb.getLinearFactor( out tmp2 );
tmp.Mult( ref tmp2, out tmp3 );
solverBody.internalSetInvMass( ref tmp3 );
solverBody.m_originalBody = rb;
rb.getAngularFactor( out solverBody.m_angularFactor );
rb.getLinearFactor( out solverBody.m_linearFactor );
rb.getLinearVelocity( out solverBody.m_linearVelocity );
rb.getAngularVelocity( out solverBody.m_angularVelocity );
rb.m_totalForce.Mult( rb.m_inverseMass * timeStep, out solverBody.m_externalForceImpulse );
//= rb.getTotalForce() * rb.getInvMass() * timeStep;
rb.m_invInertiaTensorWorld.Apply( ref rb.m_totalTorque, out tmp );
tmp.Mult( timeStep, out solverBody.m_externalTorqueImpulse );
btScalar.Dbg( "Setup external impulses " + solverBody.m_externalForceImpulse + " " + solverBody.m_externalTorqueImpulse );
///solverBody.m_externalTorqueImpulse = rb.getTotalTorque() * rb.getInvInertiaTensorWorld() * timeStep;
}
else
{
solverBody.modified = false;
solverBody.m_worldTransform = btTransform.Identity;
solverBody.internalSetInvMass( ref btVector3.Zero );
solverBody.m_originalBody = null;
solverBody.m_angularFactor = btVector3.One;
solverBody.m_linearFactor = btVector3.One;
solverBody.m_linearVelocity = btVector3.Zero;
solverBody.m_angularVelocity = btVector3.Zero;
solverBody.m_externalForceImpulse = btVector3.Zero;
solverBody.m_externalTorqueImpulse = btVector3.Zero;
}
}
internal btGeneric6DofSpring2Constraint( btRigidBody rbB, ref btTransform frameInB, RotateOrder rotOrder )
: base( btObjectTypes.D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB )
{
m_frameInB = ( frameInB );
m_rotateOrder = ( rotOrder );
m_flags = ( 0 );
///not providing rigidbody A means implicitly using worldspace for body A
rbB.m_worldTransform.Apply( ref m_frameInB, out m_frameInA );
calculateTransforms();
}
public btHingeConstraint( btRigidBody rbA, btRigidBody rbB,
ref btTransform rbAFrame, ref btTransform rbBFrame, bool useReferenceFrameA = false )
: base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA, rbB )
{
Init();
m_rbAFrame = ( rbAFrame );
m_rbBFrame = ( rbBFrame );
#if _BT_USE_CENTER_LIMIT_
m_limit = new btAngularLimit();
#endif
m_useReferenceFrameA = ( useReferenceFrameA );
m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 );
}
