public btCollisionObject getBody0Internal() { global::System.IntPtr cPtr = BulletPINVOKE.btManifoldResult_getBody0Internal(swigCPtr); btCollisionObject ret = (cPtr == global::System.IntPtr.Zero) ? null : new btCollisionObject(cPtr, false); return(ret); }
public btCollisionObject getBody1() { global::System.IntPtr cPtr = BulletPINVOKE.btPersistentManifold_getBody1(swigCPtr); btCollisionObject ret = (cPtr == global::System.IntPtr.Zero) ? null : new btCollisionObject(cPtr, false); return(ret); }
public LocalConvexResult(btCollisionObject hitCollisionObject, btCollisionWorld.LocalShapeInfo localShapeInfo, btVector3 hitNormalLocal, btVector3 hitPointLocal, float hitFraction) : this(BulletPINVOKE.new_btCollisionWorld_LocalConvexResult(btCollisionObject.getCPtr(hitCollisionObject), btCollisionWorld.LocalShapeInfo.getCPtr(localShapeInfo), btVector3.getCPtr(hitNormalLocal), btVector3.getCPtr(hitPointLocal), hitFraction), true) { if (BulletPINVOKE.SWIGPendingException.Pending) { throw BulletPINVOKE.SWIGPendingException.Retrieve(); } }
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 override btPersistentManifold getNewManifold(btCollisionObject b0, btCollisionObject b1) { global::System.IntPtr cPtr = BulletPINVOKE.btCollisionDispatcher_getNewManifold(swigCPtr, btCollisionObject.getCPtr(b0), btCollisionObject.getCPtr(b1)); btPersistentManifold ret = (cPtr == global::System.IntPtr.Zero) ? null : new btPersistentManifold(cPtr, false); return(ret); }
public void contactPairTest(btCollisionObject colObjA, btCollisionObject colObjB, btCollisionWorld.ContactResultCallback resultCallback) { BulletPINVOKE.btCollisionWorld_contactPairTest(swigCPtr, btCollisionObject.getCPtr(colObjA), btCollisionObject.getCPtr(colObjB), btCollisionWorld.ContactResultCallback.getCPtr(resultCallback)); if (BulletPINVOKE.SWIGPendingException.Pending) { throw BulletPINVOKE.SWIGPendingException.Retrieve(); } }
internal void Initialize( btCollisionObject body0, btCollisionObject body1, int unused, double contactBreakingThreshold, double contactProcessingThreshold ) { base.Initialize( btObjectTypes.BT_PERSISTENT_MANIFOLD_TYPE ); m_body0 = ( body0 ); m_body1 = ( body1 ); m_cachedPoints = ( 0 ); m_contactBreakingThreshold = ( contactBreakingThreshold ); btScalar.Dbg( "breaking contact threshold set to " + contactBreakingThreshold.ToString( "g17" ) ); m_contactBreakingThresholdSquared = contactBreakingThreshold * contactBreakingThreshold; m_contactProcessingThreshold = ( contactProcessingThreshold ); }
internal void Initialize() { base.Initialize( btObjectTypes.BT_PERSISTENT_MANIFOLD_TYPE ); m_body0 = null; m_body1 = null; m_cachedPoints = ( 0 ); m_index1a = ( 0 ); }
public override float calculateTimeOfImpact(btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut) { float ret = BulletPINVOKE.btSphereSphereCollisionAlgorithm_calculateTimeOfImpact(swigCPtr, btCollisionObject.getCPtr(body0), btCollisionObject.getCPtr(body1), btDispatcherInfo.getCPtr(dispatchInfo), btManifoldResult.getCPtr(resultOut)); if (BulletPINVOKE.SWIGPendingException.Pending) { throw BulletPINVOKE.SWIGPendingException.Retrieve(); } return(ret); }
public void Initialize( btCollisionObjectWrapper parent, btCollisionShape shape, btCollisionObject collisionObject //, ref btTransform worldTransform , int partId, int index ) { m_parent = ( parent ); m_shape = ( shape ); m_collisionObject = ( collisionObject ); //m_worldTransform = ( worldTransform ); m_partId = ( partId ); m_index = ( index ); }
public override bool needsResponse(btCollisionObject body0, btCollisionObject body1) { bool ret = BulletPINVOKE.btCollisionDispatcher_needsResponse(swigCPtr, btCollisionObject.getCPtr(body0), btCollisionObject.getCPtr(body1)); return(ret); }
public virtual void removeCollisionObject(btCollisionObject collisionObject) { BulletPINVOKE.btCollisionWorld_removeCollisionObject(swigCPtr, btCollisionObject.getCPtr(collisionObject)); }
public btCollisionObjectWrapper(btCollisionObjectWrapper parent, btCollisionShape shape, btCollisionObject collisionObject, btTransform worldTransform, int partId, int index) : this(BulletPINVOKE.new_btCollisionObjectWrapper(btCollisionObjectWrapper.getCPtr(parent), btCollisionShape.getCPtr(shape), btCollisionObject.getCPtr(collisionObject), btTransform.getCPtr(worldTransform), partId, index), true) { if (BulletPINVOKE.SWIGPendingException.Pending) { throw BulletPINVOKE.SWIGPendingException.Retrieve(); } }
public static float calculateCombinedFriction(btCollisionObject body0, btCollisionObject body1) { float ret = BulletPINVOKE.btManifoldResult_calculateCombinedFriction(btCollisionObject.getCPtr(body0), btCollisionObject.getCPtr(body1)); return(ret); }
public bool checkCollideWith(btCollisionObject co) { bool ret = BulletPINVOKE.btCollisionObject_checkCollideWith(swigCPtr, btCollisionObject.getCPtr(co)); return(ret); }
//! Creates a new contact point public btPersistentManifold* newContactManifold(btCollisionObject body0,btCollisionObject body1) { m_manifoldPtr = m_dispatcher.getNewManifold(body0,body1); return m_manifoldPtr; }
protected virtual double solveGroupCacheFriendlyIterations( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlyIterations" ); { ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for( int iteration = 0; iteration < maxIterations; iteration++ ) //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); } } return 0; }
public static void rayTestSingle(btTransform rayFromTrans, btTransform rayToTrans, btCollisionObject collisionObject, btCollisionShape collisionShape, btTransform colObjWorldTransform, btCollisionWorld.RayResultCallback resultCallback) { BulletPINVOKE.btCollisionWorld_rayTestSingle(btTransform.getCPtr(rayFromTrans), btTransform.getCPtr(rayToTrans), btCollisionObject.getCPtr(collisionObject), btCollisionShape.getCPtr(collisionShape), btTransform.getCPtr(colObjWorldTransform), btCollisionWorld.RayResultCallback.getCPtr(resultCallback)); if (BulletPINVOKE.SWIGPendingException.Pending) { throw BulletPINVOKE.SWIGPendingException.Retrieve(); } }
protected virtual double solveSingleIteration( int iteration, btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_RANDMIZE_ORDER ) != 0 ) { if( true ) // uncomment this for a bit less random ((iteration & 7) == 0) { for( int j = 0; j < numNonContactPool; ++j ) { int tmp = m_orderNonContactConstraintPool[j]; int swapi = btRandInt2( j + 1 ); m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; m_orderNonContactConstraintPool[swapi] = tmp; } //contact/friction constraints are not solved more than if( iteration < infoGlobal.m_numIterations ) { for( int j = 0; j < numConstraintPool; ++j ) { int tmp = m_orderTmpConstraintPool[j]; int swapi = btRandInt2( j + 1 ); m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; m_orderTmpConstraintPool[swapi] = tmp; } for( int j = 0; j < numFrictionPool; ++j ) { int tmp = m_orderFrictionConstraintPool[j]; int swapi = btRandInt2( j + 1 ); m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; m_orderFrictionConstraintPool[swapi] = tmp; } } } } if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 ) { ///solve all joint constraints, using SIMD, if available for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ ) { btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if( iteration < constraint.m_overrideNumSolverIterations ) resolveSingleConstraintRowGenericSIMD( constraint.m_solverBodyA , constraint.m_solverBodyB, constraint ); } if( iteration < infoGlobal.m_numIterations ) { for( int j = 0; j < numConstraints; j++ ) { btTypedConstraint constraint = constraints[j + startConstraint]; if( constraint.isEnabled() ) { btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep ); btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep ); constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep ); } } ///solve all contact constraints using SIMD, if available if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS ) != 0 ) { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int multiplier = ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) ? 2 : 1; for( int c = 0; c < numPoolConstraints; c++ ) { double totalImpulse = 0; { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); totalImpulse = solveManifold.m_appliedImpulse; } bool applyFriction = true; if( applyFriction ) { { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]]; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } } } } else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS { //solve the friction constraints after all contact constraints, don't interleave them int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int j; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; //resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); gResolveSingleConstraintRowLowerLimit_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } ///solve all friction constraints, using SIMD, if available int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count; for( j = 0; j < numFrictionPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; btScalar.Dbg( "PreFriction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) ); gResolveSingleConstraintRowGeneric_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); //resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); btScalar.Dbg( "Friction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) ); } } int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count; for( j = 0; j < numRollingFrictionPoolConstraints; j++ ) { btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; gResolveSingleConstraintRowGeneric_scalar_reference( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint ); //resolveSingleConstraintRowGenericSIMD( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint ); } } } } } else { //non-SIMD version ///solve all joint constraints for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ ) { btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if( iteration < constraint.m_overrideNumSolverIterations ) resolveSingleConstraintRowGeneric( constraint.m_solverBodyA, constraint.m_solverBodyB, constraint ); } if( iteration < infoGlobal.m_numIterations ) { for( int j = 0; j < numConstraints; j++ ) { btTypedConstraint constraint = constraints[j + startConstraint]; if( constraint.isEnabled() ) { //int bodyAid = ; //int bodyBid = ; btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep ); btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep ); constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep ); } } ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; for( int j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; resolveSingleConstraintRowLowerLimit( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } ///solve all friction constraints int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count; for( int j = 0; j < numFrictionPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse ); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; resolveSingleConstraintRowGeneric( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count; for( int j = 0; j < numRollingFrictionPoolConstraints; j++ ) { btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; if( totalImpulse > (double)( 0 ) ) { double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; resolveSingleConstraintRowGeneric( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint ); } } } } return 0; }
protected virtual void solveGroupCacheFriendlySplitImpulseIterations( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { int iteration; if( infoGlobal.m_splitImpulse ) { if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 ) { for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ ) { { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int j; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; resolveSplitPenetrationSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } } } else { for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ ) { { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int j; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; resolveSplitPenetrationImpulseCacheFriendly( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold ); } } } } } }
protected virtual double solveGroupCacheFriendlySetup( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer ) { if( m_fixedBody != null ) { BulletGlobals.SolverBodyPool.Free( m_fixedBody ); m_fixedBody = null; } //m_fixedBodyId = -1; CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlySetup" ); //(void)debugDrawer; m_maxOverrideNumSolverIterations = 0; #if BT_ADDITIONAL_DEBUG //make sure that dynamic bodies exist for all (enabled)raints for (int i=0;i<numConstraints;i++) { btTypedConstraint* constraint = constraints[i]; if (constraint.isEnabled()) { if (!constraint.getRigidBodyA().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (&constraint.getRigidBodyA()==bodies[b]) { found = true; break; } } Debug.Assert(found); } if (!constraint.getRigidBodyB().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (&constraint.getRigidBodyB()==bodies[b]) { found = true; break; } } Debug.Assert(found); } } } //make sure that dynamic bodies exist for all contact manifolds for (int i=0;i<numManifolds;i++) { if (!manifoldPtr[i].getBody0().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (manifoldPtr[i].getBody0()==bodies[b]) { found = true; break; } } Debug.Assert(found); } if (!manifoldPtr[i].getBody1().isStaticOrKinematicObject()) { bool found=false; for (int b=0;b<numBodies;b++) { if (manifoldPtr[i].getBody1()==bodies[b]) { found = true; break; } } Debug.Assert(found); } } #endif //BT_ADDITIONAL_DEBUG for( int i = 0; i < numBodies; i++ ) { bodies[i].setCompanionBody( null ); } m_tmpSolverBodyPool.Capacity = ( numBodies + 1 ); m_tmpSolverBodyPool.Count = ( 0 ); //btSolverBody fixedBody = m_tmpSolverBodyPool.expand(); //initSolverBody(&fixedBody,0); //convert all bodies for( int i = 0; i < numBodies; i++ ) { //int bodyId = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep ); btRigidBody body = btRigidBody.upcast( bodies[i] ); if( body != null && body.getInvMass() != 0 ) { btSolverBody solverBody = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep ); btVector3 gyroForce = btVector3.Zero; if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT ) != 0 ) { body.computeGyroscopicForceExplicit( infoGlobal.m_maxGyroscopicForce, out gyroForce ); btVector3 tmp; body.m_invInertiaTensorWorld.ApplyInverse( ref gyroForce, out tmp ); //solverBody.m_externalTorqueImpulse -= gyroForce * body.m_invInertiaTensorWorld * infoGlobal.m_timeStep; solverBody.m_externalTorqueImpulse.SubScale( ref tmp, infoGlobal.m_timeStep, out solverBody.m_externalTorqueImpulse ); } if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD ) != 0 ) { body.computeGyroscopicImpulseImplicit_World( infoGlobal.m_timeStep, out gyroForce ); solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse ); } if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ) != 0 ) { body.computeGyroscopicImpulseImplicit_Body( infoGlobal.m_timeStep, out gyroForce ); btScalar.Dbg( "Gyroforce " + gyroForce ); solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse ); } } } if( true ) { int j; for( j = 0; j < numConstraints; j++ ) { btTypedConstraint constraint = constraints[j + startConstraint]; constraint.buildJacobian(); constraint.internalSetAppliedImpulse( 0.0f ); } } //btRigidBody rb0=0,*rb1=0; //if (1) { { int totalNumRows = 0; int i; m_tmpConstraintSizesPool.Capacity = ( numConstraints ); //calculate the total number of contraint rows for( i = 0; i < numConstraints; i++ ) { int infoNumConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows; //btTypedConstraint.btConstraintInfo1 info1 = m_tmpConstraintSizesPool[i]; btTypedConstraint.btJointFeedback fb = constraints[i + startConstraint].getJointFeedback(); if( fb != null ) { fb.m_appliedForceBodyA.setZero(); fb.m_appliedTorqueBodyA.setZero(); fb.m_appliedForceBodyB.setZero(); fb.m_appliedTorqueBodyB.setZero(); } if( constraints[i + startConstraint].isEnabled() ) { constraints[i + startConstraint].getInfo1( ref m_tmpConstraintSizesPool.InternalArray[i] ); } else { m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows = 0; m_tmpConstraintSizesPool.InternalArray[i].nub = 0; } totalNumRows += m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.Count = ( totalNumRows ); for( i = 0; i < totalNumRows; i++ ) m_tmpSolverNonContactConstraintPool[i] = BulletGlobals.SolverConstraintPool.Get(); ///setup the btSolverConstraints int currentRow = 0; for( i = 0; i < numConstraints; i++ ) { int infoConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows; if( infoConstraintRows != 0 ) { Debug.Assert( currentRow < totalNumRows ); btSolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow]; btTypedConstraint constraint = constraints[i + startConstraint]; btRigidBody rbA = constraint.getRigidBodyA(); btRigidBody rbB = constraint.getRigidBodyB(); //int solverBodyIdA = ; //int solverBodyIdB = ; btSolverBody bodyAPtr = getOrInitSolverBody( rbA, infoGlobal.m_timeStep ); btSolverBody bodyBPtr = getOrInitSolverBody( rbB, infoGlobal.m_timeStep ); int overrideNumSolverIterations = constraint.getOverrideNumSolverIterations() > 0 ? constraint.getOverrideNumSolverIterations() : infoGlobal.m_numIterations; if( overrideNumSolverIterations > m_maxOverrideNumSolverIterations ) m_maxOverrideNumSolverIterations = overrideNumSolverIterations; int j; for( j = 0; j < infoConstraintRows; j++ ) { btSolverConstraint current = m_tmpSolverNonContactConstraintPool[currentRow + j]; current.Clear(); //memset( ¤tConstraintRow[j], 0, sizeof( btSolverConstraint ) ); current.m_lowerLimit = btScalar.SIMD_NEG_INFINITY; current.m_upperLimit = btScalar.SIMD_INFINITY; current.m_appliedImpulse = 0; current.m_appliedPushImpulse = 0; current.m_solverBodyA = bodyAPtr; current.m_solverBodyB = bodyBPtr; current.m_overrideNumSolverIterations = overrideNumSolverIterations; } bodyAPtr.Clear(); btTypedConstraint.btConstraintInfo2 info2 = m_tmpConstraintInfo2Pool.Get();// new btTypedConstraint.btConstraintInfo2(); info2.m_numRows = infoConstraintRows; for( j = 0; j < infoConstraintRows; ++j ) { info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j]; } info2.fps = 1 / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; #if OLD_CONSTRAINT_INFO_INIT info2.m_J1linearAxis = currentConstraintRow.m_contactNormal1; info2.m_J1angularAxis = currentConstraintRow.m_relpos1CrossNormal; info2.m_J2linearAxis = currentConstraintRow.m_contactNormal2; info2.m_J2angularAxis = currentConstraintRow.m_relpos2CrossNormal; info2.rowskip = 0;// sizeof( btSolverConstraint ) / sizeof( double );//check this ///the size of btSolverConstraint needs be a multiple of double //Debug.Assert( info2.rowskip * sizeof( double ) == sizeof( btSolverConstraint ) ); info2.m_constraintError = currentConstraintRow.m_rhs; info2.cfm = currentConstraintRow.m_cfm; info2.m_lowerLimit = currentConstraintRow.m_lowerLimit; info2.m_upperLimit = currentConstraintRow.m_upperLimit; #endif currentConstraintRow.m_cfm = infoGlobal.m_globalCfm; info2.m_damping = infoGlobal.m_damping; info2.m_numIterations = infoGlobal.m_numIterations; constraint.getInfo2( info2 ); ///finalize the constraint setup for( j = 0; j < infoConstraintRows; j++ ) { btSolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j]; if( solverConstraint.m_upperLimit >= constraint.getBreakingImpulseThreshold() ) { solverConstraint.m_upperLimit = constraint.getBreakingImpulseThreshold(); } if( solverConstraint.m_lowerLimit <= -constraint.getBreakingImpulseThreshold() ) { solverConstraint.m_lowerLimit = -constraint.getBreakingImpulseThreshold(); } solverConstraint.m_originalContactPoint = constraint; btVector3 tmp; { //solverConstraint.m_angularComponentA = constraint.getRigidBodyA().m_invInertiaTensorWorld // *solverConstraint.m_relpos1CrossNormal * constraint.getRigidBodyA().getAngularFactor(); constraint.m_rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp ); tmp.Mult( ref constraint.m_rbA.m_angularFactor, out solverConstraint.m_angularComponentA ); } { //solverConstraint.m_angularComponentB = constraint.getRigidBodyB().m_invInertiaTensorWorld // * solverConstraint.m_relpos2CrossNormal * constraint.getRigidBodyB().getAngularFactor(); constraint.m_rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp ); tmp.Mult( ref constraint.m_rbB.m_angularFactor, out solverConstraint.m_angularComponentB ); } { btVector3 iMJlA; solverConstraint.m_contactNormal1.Mult( rbA.m_inverseMass, out iMJlA ); btVector3 iMJaA; rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out iMJaA ); btVector3 iMJlB; solverConstraint.m_contactNormal2.Mult( rbB.m_inverseMass, out iMJlB );//sign of normal? btVector3 iMJaB; rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out iMJaB ); double sum = iMJlA.dot( ref solverConstraint.m_contactNormal1 ); sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal ); sum += iMJlB.dot( ref solverConstraint.m_contactNormal2 ); sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal ); double fsum = btScalar.btFabs( sum ); Debug.Assert( fsum > btScalar.SIMD_EPSILON ); btScalar.Dbg( "m_jacDiagABInv 4 set to " + ( fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0 ).ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0; } { double rel_vel; btVector3 externalForceImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalTorqueImpulse : btVector3.Zero; btVector3 externalForceImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalTorqueImpulse : btVector3.Zero; btScalar.Dbg( "external torque2 impulses " + externalTorqueImpulseA + externalTorqueImpulseB ); double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref rbA.m_linearVelocity, ref externalForceImpulseA ) + solverConstraint.m_relpos1CrossNormal.dotAdded( ref rbA.m_angularVelocity, ref externalTorqueImpulseA ); double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref rbB.m_linearVelocity, ref externalForceImpulseB ) + solverConstraint.m_relpos2CrossNormal.dotAdded( ref rbB.m_angularVelocity, ref externalTorqueImpulseB ); rel_vel = vel1Dotn + vel2Dotn; double restitution = 0; double positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 double velocityError = restitution - rel_vel * info2.m_damping; double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; btScalar.Dbg( "Constraint 5 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_appliedImpulse = 0; } } } currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows; } } btScalar.Dbg( "About to convert contacts " + start_manifold + " " + numManifolds ); convertContacts( manifoldPtr, start_manifold, numManifolds, infoGlobal ); } // btContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderNonContactConstraintPool.Capacity = ( numNonContactPool ); if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool * 2 ); else m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool ); m_orderFrictionConstraintPool.Count = m_orderFrictionConstraintPool.Capacity = ( numFrictionPool ); { int i; for( i = 0; i < numNonContactPool; i++ ) { m_orderNonContactConstraintPool[i] = i; } for( i = 0; i < numConstraintPool; i++ ) { m_orderTmpConstraintPool[i] = i; } for( i = 0; i < numFrictionPool; i++ ) { m_orderFrictionConstraintPool[i] = i; } } return 0; }
public virtual void addCollisionObject(btCollisionObject collisionObject, short collisionFilterGroup) { BulletPINVOKE.btCollisionWorld_addCollisionObject__SWIG_1(swigCPtr, btCollisionObject.getCPtr(collisionObject), collisionFilterGroup); }
internal override double calculateTimeOfImpact( btCollisionObject col0, btCollisionObject col1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { //(void)resultOut; //(void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0.m_worldTransform, double resultFraction = btScalar.BT_ONE; btVector3 tmp; col0.m_interpolationWorldTransform.m_origin.Sub( ref col0.m_worldTransform.m_origin, out tmp ); double squareMot0 = ( tmp ).length2(); col1.m_interpolationWorldTransform.m_origin.Sub( ref col1.m_worldTransform.m_origin, out tmp ); double squareMot1 = ( tmp ).length2(); if( squareMot0 < col0.getCcdSquareMotionThreshold() && squareMot1 < col1.getCcdSquareMotionThreshold() ) return resultFraction; if( disableCcd ) return btScalar.BT_ONE; //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) /// Convex0 against sphere for Convex1 { btConvexShape convex0 = (btConvexShape)col0.getCollisionShape(); using( btSphereShape sphere1 = BulletGlobals.SphereShapePool.Get() ) { sphere1.Initialize( col1.getCcdSweptSphereRadius() ); //todo: allow non-zero sphere sizes, for better approximation btConvexCast.CastResult result = BulletGlobals.CastResultPool.Get(); btVoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get(); //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get(); ccd1.Initialize( convex0, sphere1, voronoiSimplex ); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if( ccd1.calcTimeOfImpact( ref col0.m_worldTransform, ref col0.m_interpolationWorldTransform, ref col1.m_worldTransform, ref col1.m_interpolationWorldTransform, result ) ) { //store result.m_fraction in both bodies if( col0.getHitFraction() > result.m_fraction ) col0.setHitFraction( result.m_fraction ); if( col1.getHitFraction() > result.m_fraction ) col1.setHitFraction( result.m_fraction ); if( resultFraction > result.m_fraction ) resultFraction = result.m_fraction; } BulletGlobals.GjkConvexCastPool.Free( ccd1 ); } } /// Sphere (for convex0) against Convex1 { btConvexShape convex1 = (btConvexShape)( col1.getCollisionShape() ); using( btSphereShape sphere0 = BulletGlobals.SphereShapePool.Get() ) { sphere0.Initialize( col0.getCcdSweptSphereRadius() ); //todo: allow non-zero sphere sizes, for better approximation btConvexCast.CastResult result = BulletGlobals.CastResultPool.Get(); btVoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get(); //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get(); ccd1.Initialize( sphere0, convex1, voronoiSimplex ); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if( ccd1.calcTimeOfImpact( ref col0.m_worldTransform, ref col0.m_interpolationWorldTransform, ref col1.m_worldTransform, ref col1.m_interpolationWorldTransform, result ) ) { //store result.m_fraction in both bodies if( col0.getHitFraction() > result.m_fraction ) col0.setHitFraction( result.m_fraction ); if( col1.getHitFraction() > result.m_fraction ) col1.setHitFraction( result.m_fraction ); if( resultFraction > result.m_fraction ) resultFraction = result.m_fraction; } BulletGlobals.GjkConvexCastPool.Free( ccd1 ); } } return resultFraction; }
internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { Debug.Assert( false ); //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures btCollisionObject colObj = m_isSwapped ? body1 : body0; btCollisionObject otherObj = m_isSwapped ? body0 : body1; Debug.Assert( colObj.getCollisionShape().isCompound() ); btCompoundShape compoundShape = (btCompoundShape)( colObj.getCollisionShape() ); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 //then use each overlapping node AABB against Tree0 //and vise versa. double hitFraction = btScalar.BT_ONE; int numChildren = m_childCollisionAlgorithms.Count; int i; //btTransform orgTrans; double frac; for( i = 0; i < numChildren; i++ ) { //btCollisionShape childShape = compoundShape.getChildShape(i); //backup //orgTrans = colObj.m_worldTransform; btTransform childTrans = compoundShape.getChildTransform( i ); //btTransform newChildWorldTrans = orgTrans*childTrans ; //colObj.setWorldTransform( orgTrans * childTrans ); btTransform tmp; colObj.m_worldTransform.Apply( ref childTrans, out tmp ); colObj.m_worldTransform = tmp; //btCollisionShape tmpShape = colObj.getCollisionShape(); //colObj.internalSetTemporaryCollisionShape( childShape ); frac = m_childCollisionAlgorithms[i].calculateTimeOfImpact( colObj, otherObj, dispatchInfo, resultOut ); if( frac < hitFraction ) { hitFraction = frac; } //revert back //colObj.internalSetTemporaryCollisionShape( tmpShape); colObj.setWorldTransform( ref colObj.m_worldTransform ); } return hitFraction; }
protected virtual double solveGroupCacheFriendlyFinish( btCollisionObject[] bodies, int numBodies, btContactSolverInfo infoGlobal ) { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int i, j; if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 ) { for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[j]; btManifoldPoint pt = solveManifold.m_originalContactPoint as btManifoldPoint; Debug.Assert( pt != null ); pt.m_appliedImpulse = solveManifold.m_appliedImpulse; // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; // Console.WriteLine("pt.m_appliedImpulseLateral1 = %f\n", f); pt.m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; btScalar.Dbg( "New manifold source is " + pt.m_appliedImpulseLateral1.ToString( "g17" ) + " from " + solveManifold.m_frictionIndex ); //Console.WriteLine("pt.m_appliedImpulseLateral1 = %f\n", pt.m_appliedImpulseLateral1); if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) { pt.m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; } //do a callback here? } } numPoolConstraints = m_tmpSolverNonContactConstraintPool.Count; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint constr = (btTypedConstraint)solverConstr.m_originalContactPoint; btTypedConstraint.btJointFeedback fb = constr.getJointFeedback(); if( fb != null ) { double scalar = solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; btVector3 tmp; solverConstr.m_contactNormal1.Mult2( ref constr.m_rbA.m_linearFactor, scalar, out tmp ); fb.m_appliedForceBodyA.Add( ref tmp, out fb.m_appliedForceBodyA ); //fb.m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr.getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep; solverConstr.m_contactNormal2.Mult2( ref constr.m_rbB.m_linearFactor, scalar, out tmp ); fb.m_appliedForceBodyB.Add( ref tmp, out fb.m_appliedForceBodyB ); //fb.m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr.getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep; solverConstr.m_relpos1CrossNormal.Mult2( ref constr.m_rbA.m_angularFactor, scalar, out tmp ); fb.m_appliedTorqueBodyA.Add( ref tmp, out fb.m_appliedTorqueBodyA ); //fb.m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr.getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; solverConstr.m_relpos2CrossNormal.Mult2( ref constr.m_rbB.m_angularFactor, scalar, out tmp ); fb.m_appliedTorqueBodyB.Add( ref tmp, out fb.m_appliedTorqueBodyB ); //fb.m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr.getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */ } constr.internalSetAppliedImpulse( solverConstr.m_appliedImpulse ); if( btScalar.btFabs( solverConstr.m_appliedImpulse ) >= constr.getBreakingImpulseThreshold() ) { constr.setEnabled( false ); } } for( i = 0; i < m_tmpSolverBodyPool.Count; i++ ) { btSolverBody solverBody = m_tmpSolverBodyPool[i]; btRigidBody body = solverBody.m_originalBody; if( body != null ) { if( infoGlobal.m_splitImpulse ) solverBody.writebackVelocityAndTransform( infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp ); else solverBody.writebackVelocity(); btVector3 tmp; solverBody.m_linearVelocity.Add( ref solverBody.m_externalForceImpulse, out tmp ); solverBody.m_originalBody.setLinearVelocity( ref tmp ); solverBody.m_angularVelocity.Add( ref solverBody.m_externalTorqueImpulse, out tmp ); solverBody.m_originalBody.setAngularVelocity( ref tmp ); if( infoGlobal.m_splitImpulse && solverBody.modified ) { btScalar.Dbg( DbgFlag.PredictedTransform, "Solver body transform is " + solverBody.m_worldTransform ); solverBody.m_originalBody.setWorldTransform( ref solverBody.m_worldTransform ); } BulletGlobals.SolverBodyPool.Free( solverBody ); body.setCompanionBody( null ); } } foreach( btSolverConstraint constraint in m_tmpSolverContactConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); foreach( btSolverConstraint constraint in m_tmpSolverNonContactConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); foreach( btSolverConstraint constraint in m_tmpSolverContactFrictionConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); foreach( btSolverConstraint constraint in m_tmpSolverContactRollingFrictionConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); m_tmpSolverContactConstraintPool.Count = 0; // resizeNoInitialize( 0 ); m_tmpSolverNonContactConstraintPool.Count = 0; //resizeNoInitialize( 0 ); m_tmpSolverContactFrictionConstraintPool.Count = 0; //resizeNoInitialize( 0 ); m_tmpSolverContactRollingFrictionConstraintPool.Count = 0; //resizeNoInitialize( 0 ); m_tmpSolverBodyPool.Count = 0; //resizeNoInitialize( 0 ); m_fixedBody = null; return 0; }
public override void addCollisionObject(btCollisionObject collisionObject) { BulletPINVOKE.btDiscreteDynamicsWorld_addCollisionObject__SWIG_2(swigCPtr, btCollisionObject.getCPtr(collisionObject)); }
public void Initialize( btCollisionObject me, ref btVector3 fromA, ref btVector3 toA, btOverlappingPairCache pairCache, btDispatcher dispatcher ) { base.Initialize( ref fromA, ref toA ); m_me = ( me ); m_allowedPenetration = ( 0.0f ); m_pairCache = ( pairCache ); m_dispatcher = ( dispatcher ); }
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(btCollisionObject obj) { return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr); }
StaticOnlyCallback( btCollisionObject me, ref btVector3 fromA, ref btVector3 toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher ) :
double calculateTimeOfImpact(btCollisionObject body0,btCollisionObject body1,btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
public override void removeCollisionObject( btCollisionObject collisionObject ) { btRigidBody body = btRigidBody.upcast( collisionObject ); if( body != null ) removeRigidBody( body ); else base.removeCollisionObject( collisionObject ); }
public abstract btBroadphaseProxy createProxy( ref btVector3 aabbMin, ref btVector3 aabbMax, BroadphaseNativeTypes shapeType, btCollisionObject userPtr, btBroadphaseProxy.CollisionFilterGroups collisionFilterGroup, btBroadphaseProxy.CollisionFilterGroups collisionFilterMask, btDispatcher dispatcher, object multiSapProxy );
public void setBodies(btCollisionObject body0, btCollisionObject body1) { BulletPINVOKE.btPersistentManifold_setBodies(swigCPtr, btCollisionObject.getCPtr(body0), btCollisionObject.getCPtr(body1)); }
public void updateSingleAabb(btCollisionObject colObj) { BulletPINVOKE.btCollisionWorld_updateSingleAabb(swigCPtr, btCollisionObject.getCPtr(colObj)); }
public static void objectQuerySingle(btConvexShape castShape, btTransform rayFromTrans, btTransform rayToTrans, btCollisionObject collisionObject, btCollisionShape collisionShape, btTransform colObjWorldTransform, btCollisionWorld.ConvexResultCallback resultCallback, float allowedPenetration) { BulletPINVOKE.btCollisionWorld_objectQuerySingle(btConvexShape.getCPtr(castShape), btTransform.getCPtr(rayFromTrans), btTransform.getCPtr(rayToTrans), btCollisionObject.getCPtr(collisionObject), btCollisionShape.getCPtr(collisionShape), btTransform.getCPtr(colObjWorldTransform), btCollisionWorld.ConvexResultCallback.getCPtr(resultCallback), allowedPenetration); if (BulletPINVOKE.SWIGPendingException.Pending) { throw BulletPINVOKE.SWIGPendingException.Retrieve(); } }
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject void removeCollisionObject( btCollisionObject collisionObject ) { btRigidBody body = collisionObject as btRigidBody; if( body != null ) removeRigidBody( body ); else btCollisionWorld::removeCollisionObject( collisionObject ); }
internal abstract bool needsCollision( btCollisionObject body0, btCollisionObject body1 );
/// btSequentialImpulseConstraintSolver Sequentially applies impulses internal override double solveGroup( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo infoGlobal , btIDebugDraw debugDrawer, btDispatcher dispatcher ) { CProfileSample sample = new CProfileSample( "solveGroup" ); //you need to provide at least some bodies solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); solveGroupCacheFriendlyIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer ); solveGroupCacheFriendlyFinish( bodies, numBodies, infoGlobal ); return 0; }
public virtual void addCollisionObject(btCollisionObject collisionObject) { BulletPINVOKE.btCollisionWorld_addCollisionObject__SWIG_2(swigCPtr, btCollisionObject.getCPtr(collisionObject)); }
internal override void processIsland( btCollisionObject[] bodies, int numBodies, btPersistentManifold[] manifolds, int first_manifold, int numManifolds, int islandId ) { if( islandId < 0 ) { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id m_solver.solveGroup( bodies, numBodies, manifolds, first_manifold, numManifolds, m_sortedConstraints, 0, m_numConstraints, m_solverInfo, m_debugDrawer, m_dispatcher ); } else { //also add all non-contact constraints/joints for this island int startConstraint = 0; int numCurConstraints = 0; int i; //find the first constraint for this island for( i = 0; i < m_numConstraints; i++ ) { if( btDiscreteDynamicsWorld.btGetConstraintIslandId( m_sortedConstraints[i] ) == islandId ) { startConstraint = i; break; } } //count the number of constraints in this island for( ; i < m_numConstraints; i++ ) { if( btDiscreteDynamicsWorld.btGetConstraintIslandId( m_sortedConstraints[i] ) == islandId ) { numCurConstraints++; } } if( m_solverInfo.m_minimumSolverBatchSize <= 1 ) { m_solver.solveGroup( bodies, numBodies, manifolds, first_manifold, numManifolds, m_sortedConstraints, startConstraint, numCurConstraints, m_solverInfo, m_debugDrawer, m_dispatcher ); } else { for( i = 0; i < numBodies; i++ ) m_bodies.Add( bodies[i] ); for( i = 0; i < numManifolds; i++ ) m_manifolds.Add( manifolds[first_manifold + i] ); for( i = 0; i < numCurConstraints; i++ ) m_constraints.Add( m_sortedConstraints[startConstraint + i] ); if( ( m_constraints.Count + m_manifolds.Count ) > m_solverInfo.m_minimumSolverBatchSize ) { processConstraints(); } else { //Console.WriteLine("deferred\n"); } } } }
protected void applyAnisotropicFriction( btCollisionObject colObj, ref btVector3 frictionDirection , btCollisionObject.AnisotropicFrictionFlags frictionMode ) { if( colObj != null && colObj.hasAnisotropicFriction( frictionMode ) ) { // transform to local coordinates btVector3 loc_lateral;// = frictionDirection * colObj.getWorldTransform().getBasis(); colObj.m_worldTransform.m_basis.Apply( ref frictionDirection, out loc_lateral ); //btVector3 friction_scaling; = colObj.getAnisotropicFriction(); //apply anisotropic friction loc_lateral.Mult( ref colObj.m_anisotropicFriction, out loc_lateral ); // ... and transform it back to global coordinates colObj.m_worldTransform.m_basis.Apply( ref loc_lateral, out frictionDirection ); //frictionDirection = colObj.getWorldTransform().getBasis() * loc_lateral; } }
public override void addCollisionObject( btCollisionObject collisionObject, btBroadphaseProxy.CollisionFilterGroups collisionFilterGroup = btBroadphaseProxy.CollisionFilterGroups.StaticFilter , btBroadphaseProxy.CollisionFilterGroups collisionFilterMask = btBroadphaseProxy.CollisionFilterGroups.StaticFilter ) { base.addCollisionObject( collisionObject, collisionFilterGroup, collisionFilterMask ); }
internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis //, int solverBodyIdA, int solverBodyIdB , btSolverBody solverBodyA, btSolverBody solverBodyB , btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 ) { //btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; //btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody body0 = solverBodyA.m_originalBody; btRigidBody body1 = solverBodyB.m_originalBody; solverConstraint.m_solverBodyA = solverBodyA; solverConstraint.m_solverBodyB = solverBodyB; solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = null; solverConstraint.m_appliedImpulse = 0; solverConstraint.m_appliedPushImpulse = 0; if( body0 != null ) { solverConstraint.m_contactNormal1 = normalAxis; rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal ); btVector3 tmp; body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp ); tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA ); } else { solverConstraint.m_contactNormal1.setZero(); solverConstraint.m_relpos1CrossNormal.setZero(); solverConstraint.m_angularComponentA.setZero(); } if( body1 != null ) { normalAxis.Invert( out solverConstraint.m_contactNormal2 ); rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal ); btVector3 tmp; body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp ); tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB ); } else { solverConstraint.m_contactNormal2 = btVector3.Zero; solverConstraint.m_relpos2CrossNormal = btVector3.Zero; solverConstraint.m_angularComponentB = btVector3.Zero; } { btVector3 vec; double denom0 = 0; double denom1 = 0; if( body0 != null ) { solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec ); denom0 = body0.getInvMass() + normalAxis.dot( ref vec ); } if( body1 != null ) { btVector3 tmp; solverConstraint.m_angularComponentB.Invert( out tmp ); tmp.cross( ref rel_pos2, out vec ); denom1 = body1.getInvMass() + normalAxis.dot( ref vec ); } double denom = relaxation / ( denom0 + denom1 ); btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = denom; } { double rel_vel; double vel1Dotn; double vel2Dotn; //double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero ); if( body0 != null ) vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse ) + solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity ); else vel1Dotn = 0; //double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero ); if( body1 != null ) vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse ) + solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity ); else vel2Dotn = 0; rel_vel = vel1Dotn + vel2Dotn; // double positionalError = 0; double velocityError = desiredVelocity - rel_vel; double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = velocityImpulse; btScalar.Dbg( "Constraint 1 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_rhsPenetration = 0; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } }
internal btSolverConstraint addFrictionConstraint( ref btVector3 normalAxis //, int solverBodyIdA, int solverBodyIdB , btSolverBody solverBodyA, btSolverBody solverBodyB , int frictionIndex, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0 ) { btSolverConstraint solverConstraint; m_tmpSolverContactFrictionConstraintPool.Add( solverConstraint = BulletGlobals.SolverConstraintPool.Get() ); solverConstraint.m_frictionIndex = frictionIndex; setupFrictionConstraint( solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip ); btScalar.Dbg( "Setup Impulse2 = " + solverConstraint.m_appliedImpulse.ToString( "g17" ) ); return solverConstraint; }
internal void setupRollingFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis1 , btSolverBody solverBodyA, btSolverBody solverBodyB, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 ) { btVector3 normalAxis = btVector3.Zero; solverConstraint.m_contactNormal1 = normalAxis; normalAxis.Invert( out solverConstraint.m_contactNormal2 ); //btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; //btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody body0 = solverBodyA.m_originalBody; btRigidBody body1 = solverBodyB.m_originalBody; solverConstraint.m_solverBodyA = solverBodyA; solverConstraint.m_solverBodyB = solverBodyB; solverConstraint.m_friction = cp.m_combinedRollingFriction; solverConstraint.m_originalContactPoint = null; solverConstraint.m_appliedImpulse = 0; solverConstraint.m_appliedPushImpulse = 0; btVector3 iMJaA; btVector3 iMJaB; { btVector3 ftorqueAxis1; normalAxis1.Invert( out ftorqueAxis1 ); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; if( body0 != null ) { body0.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaA ); iMJaA.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA ); } else iMJaA = btVector3.Zero; //solverConstraint.m_angularComponentA = body0 != null ? body0.getInvInertiaTensorWorld() * ftorqueAxis1 * body0.getAngularFactor() : btVector3.Zero; } { btVector3 ftorqueAxis1 = normalAxis1; solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; if( body1 != null ) { body1.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaB ); iMJaB.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB ); } else iMJaB = btVector3.Zero; //solverConstraint.m_angularComponentB = body1 != null ? body1.getInvInertiaTensorWorld() * ftorqueAxis1 * body1.getAngularFactor() : btVector3.Zero; } { //btVector3 iMJaA = body0 != null ? body0.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3.Zero; //btVector3 iMJaB = body1 != null ? body1.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3.Zero; double sum = 0; sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal ); sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal ); btScalar.Dbg( "m_jacDiagABInv 2 set to " + ( btScalar.BT_ONE / sum ).ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = btScalar.BT_ONE / sum; } { double rel_vel; double vel1Dotn; double vel2Dotn; //double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero ); //double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero ); if( body0 != null ) vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse ) + solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity ); else vel1Dotn = 0; if( body1 != null ) vel2Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse ) + solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyB.m_angularVelocity ); else vel2Dotn = 0; rel_vel = vel1Dotn + vel2Dotn; // double positionalError = 0; double velocityError = desiredVelocity - rel_vel; double velocityImpulse = velocityError * ( solverConstraint.m_jacDiagABInv ); solverConstraint.m_rhs = velocityImpulse; btScalar.Dbg( "Constraint 2 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } }
public btPersistentManifold(btCollisionObject body0, btCollisionObject body1, int arg2, float contactBreakingThreshold, float contactProcessingThreshold) : this(BulletPINVOKE.new_btPersistentManifold__SWIG_1(btCollisionObject.getCPtr(body0), btCollisionObject.getCPtr(body1), arg2, contactBreakingThreshold, contactProcessingThreshold), true) { }
internal btSolverConstraint addRollingFrictionConstraint( ref btVector3 normalAxis , btSolverBody solverBodyA, btSolverBody solverBodyB , int frictionIndex, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0 ) { btSolverConstraint solverConstraint; m_tmpSolverContactRollingFrictionConstraintPool.Add( solverConstraint = BulletGlobals.SolverConstraintPool.Get() ); solverConstraint.m_frictionIndex = frictionIndex; setupRollingFrictionConstraint( solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip ); return solverConstraint; }
internal abstract btPersistentManifold getNewManifold( btCollisionObject b0, btCollisionObject b1 );
protected btSolverBody getOrInitSolverBody( btCollisionObject body, double timeStep ) { btSolverBody solverBodyA = null; if( body.getCompanionBody() != null ) { //body has already been converted solverBodyA = body.getCompanionBody(); //Debug.Assert( solverBodyIdA < m_tmpSolverBodyPool.Count ); } else { btRigidBody rb = btRigidBody.upcast( body ); //convert both active and kinematic objects (for their velocity) if( rb != null && ( rb.getInvMass() != 0 || rb.isKinematicObject() ) ) { //solverBodyA = m_tmpSolverBodyPool.Count; //btSolverBody solverBodyA; m_tmpSolverBodyPool.Add( solverBodyA = BulletGlobals.SolverBodyPool.Get() ); initSolverBody( solverBodyA, rb, timeStep ); body.setCompanionBody( solverBodyA ); } else { if( m_fixedBody == null ) { //m_fixedBodyId = m_tmpSolverBodyPool.Count; //btSolverBody fixedBody; m_tmpSolverBodyPool.Add( m_fixedBody = BulletGlobals.SolverBodyPool.Get() ); initSolverBody( m_fixedBody, null, timeStep ); } return m_fixedBody; // return 0;//assume first one is a fixed solver body } } return solverBodyA; }
internal abstract bool needsResponse( btCollisionObject body0, btCollisionObject body1 );
internal override double calculateTimeOfImpact( btCollisionObject a, btCollisionObject b, btDispatcherInfo c , btManifoldResult d ) { return btScalar.BT_ONE; }
public override void addCollisionObject(btCollisionObject collisionObject, short collisionFilterGroup) { BulletPINVOKE.btDiscreteDynamicsWorld_addCollisionObject__SWIG_1(swigCPtr, btCollisionObject.getCPtr(collisionObject), collisionFilterGroup); }
public void setIgnoreCollisionCheck(btCollisionObject co, bool ignoreCollisionCheck) { BulletPINVOKE.btCollisionObject_setIgnoreCollisionCheck(swigCPtr, btCollisionObject.getCPtr(co), ignoreCollisionCheck); }
public override void removeCollisionObject(btCollisionObject collisionObject) { BulletPINVOKE.btDiscreteDynamicsWorld_removeCollisionObject(swigCPtr, btCollisionObject.getCPtr(collisionObject)); }
public virtual bool needsCollision(btCollisionObject body0, btCollisionObject body1) { bool ret = BulletPINVOKE.btDispatcher_needsCollision(swigCPtr, btCollisionObject.getCPtr(body0), btCollisionObject.getCPtr(body1)); return(ret); }