btStaticPlaneShape( ref btVector3 planeOrigin, ref btVector3 planeNormal ) : base() { planeNormal.normalized( out m_planeNormal ); m_planeConstant = planeOrigin.dot( ref planeNormal ); m_localScaling = btVector3.Zero; m_shapeType = BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE; // Debug.Assert( btFuzzyZero(m_planeNormal.length() - btScalar.BT_ONE) ); }
void setAxis( ref btVector3 axis1, ref btVector3 axis2 ) { btVector3 zAxis; axis1.normalized( out zAxis ); btVector3 yAxis; axis2.normalized( out yAxis ); btVector3 xAxis; yAxis.cross( ref zAxis, out xAxis ); // we want right coordinate system btTransform frameInW = btTransform.Identity; frameInW.m_basis.setValue( ref xAxis, ref yAxis, ref zAxis ); // now get constraint frame in local coordinate systems btTransform tmp; m_rbA.m_worldTransform.inverse( out tmp ); tmp.Apply( ref frameInW, out m_frameInA ); m_rbB.m_worldTransform.inverse( out tmp ); tmp.Apply( ref frameInW, out m_frameInB ); calculateTransforms(); }
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping public void applyDamping( double timeStep ) { //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway //#define USE_OLD_DAMPING_METHOD 1 #if USE_OLD_DAMPING_METHOD m_linearVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_linearDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) ); m_angularVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_angularDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) ); #else m_linearVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep ), out m_linearVelocity ); m_angularVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep ), out m_angularVelocity ); //m_linearVelocity *= btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep ); //m_angularVelocity *= btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep ); #endif if( m_additionalDamping ) { //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete if( ( m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr ) & ( m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr ) ) { m_linearVelocity.Mult( m_additionalDampingFactor, out m_linearVelocity ); m_angularVelocity.Mult( m_additionalDampingFactor, out m_angularVelocity ); //m_angularVelocity *= m_additionalDampingFactor; //m_linearVelocity *= m_additionalDampingFactor; } double speed = m_linearVelocity.length(); if( speed < m_linearDamping ) { double dampVel = (double)( 0.005 ); if( speed > dampVel ) { btVector3 dir; m_linearVelocity.normalized( out dir ); dir.Mult( dampVel, out dir ); m_linearVelocity.Sub( ref dir, out m_linearVelocity ); //m_linearVelocity -= dir * dampVel; } else { m_linearVelocity = btVector3.Zero; } } double angSpeed = m_angularVelocity.length(); if( angSpeed < m_angularDamping ) { double angDampVel = (double)( 0.005 ); if( angSpeed > angDampVel ) { btVector3 dir; m_angularVelocity.normalized( out dir ); dir.Mult( angDampVel, out dir ); m_angularVelocity.Sub( ref dir, out m_angularVelocity ); //m_angularVelocity -= dir * angDampVel; } else { m_angularVelocity = btVector3.Zero; } } } }
void setAxis( ref btVector3 axis1, ref btVector3 axis2 ) { btVector3 zAxis; axis1.normalized( out zAxis ); btVector3 yAxis; axis2.normalized( out yAxis ); btVector3 xAxis; yAxis.cross( ref zAxis, out xAxis ); // we want right coordinate system btTransform frameInW = btTransform.Identity; //frameInW.setIdentity(); frameInW.m_basis.setValue( xAxis[0], yAxis[0], zAxis[0], xAxis[1], yAxis[1], zAxis[1], xAxis[2], yAxis[2], zAxis[2] ); // now get constraint frame in local coordinate systems btTransform inv; m_rbA.m_worldTransform.inverse( out inv ); inv.Apply( ref frameInW, out m_frameInA ); m_rbB.m_worldTransform.inverse( out inv ); inv.Apply( ref frameInW, out m_frameInB ); //m_frameInB = m_rbB.m_worldTransform.inverse() * frameInW; calculateTransforms(); }
public void shortestArcQuatNormalize2( ref btVector3 v0, ref btVector3 v1, out btQuaternion result ) { btVector3 _v0; btVector3 _v1; v0.normalized( out _v0 ); v1.normalized( out _v1 ); shortestArcQuat( ref _v0, ref _v1, out result ); }
static void Main(string[] args) { btVector3 testvec = new btVector3(-2, 1, 0); Console.WriteLine(String.Format("Original: {0}", testvec.testStr())); btVector3 testvec2 = testvec.absolute(); Console.WriteLine(String.Format("absolute: {0}", testvec2.testStr())); Console.WriteLine(String.Format("angle:{0}", testvec.angle(testvec2))); Console.WriteLine(String.Format("closestAxis(orig):{0}", testvec.closestAxis())); btVector3 testvec3 = testvec.cross(testvec2); Console.WriteLine(String.Format("cross: {0}", testvec3.testStr())); Console.WriteLine(String.Format("distance: {0}", testvec.distance(testvec2))); Console.WriteLine(String.Format("distance2: {0}", testvec.distance2(testvec2))); Console.WriteLine(String.Format("dot: {0}", testvec.dot(testvec2))); Console.WriteLine(String.Format("furthestAxis(orig): {0}", testvec.furthestAxis())); btVector3 testvec4 = testvec.normalized(); Console.WriteLine(String.Format("normalized: {0}", testvec4.testStr())); testvec4.setInterpolate3(testvec, testvec2, 0.5f); Console.WriteLine(String.Format("interpolate3: {0}", testvec4.testStr())); testvec4.setValue(7f, -0.09f, 2.5f); Console.WriteLine(String.Format("setvec: {0}", testvec4.testStr())); testvec4.setX(5.0f); testvec4.setY(-0.25f); testvec4.setZ(90f); testvec.setValue(0, 0, -1024); testvec2.setValue(256, 256, 1024); Console.WriteLine(String.Format("setvecIndividual: {0}", testvec4.testStr())); btAxisSweep3 testbtAxisSweep3 = new btAxisSweep3(testvec, testvec2, 50); btDefaultCollisionConfiguration colconfig = new btDefaultCollisionConfiguration(); btCollisionDispatcher coldisp = new btCollisionDispatcher(colconfig); btSequentialImpulseConstraintSolver seqimpconssol = new btSequentialImpulseConstraintSolver(); btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(coldisp, testbtAxisSweep3, seqimpconssol, colconfig); dynamicsWorld.setGravity(new btVector3(0, 0, -9.87f)); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); btQuaternion testquat = new btQuaternion(50, 0, 0, 1); btQuaternion testquatnorm = testquat.normalized(); Console.WriteLine(String.Format("testquat: {0}", testquat.testStr())); Console.WriteLine(String.Format("testquatnormalize: {0}", testquatnorm.testStr())); Console.WriteLine(String.Format("testquatLength: {0}", testquat.length())); Console.WriteLine(String.Format("testquatnormalizeLength: {0}", testquatnorm.length())); float[] heightdata = new float[256 * 256]; for (int j = 0; j < 256 * 256; j++) { if (j % 2 == 0) { heightdata[j] = 21f; } else { heightdata[j] = 28f; } } btHeightfieldTerrainShape obj = new btHeightfieldTerrainShape(256, 256, heightdata, 1.0f, 0, 256, (int)btHeightfieldTerrainShape.UPAxis.Z, (int)btHeightfieldTerrainShape.PHY_ScalarType. PHY_FLOAT, false); btCapsuleShape cap = new btCapsuleShape(0.23f, 3); btTriangleMesh testMesh = new btTriangleMesh(true, false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, 0, 1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(-1, 0, -1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(1, 0, 1), false); btGImpactMeshShape meshtest = new btGImpactMeshShape(testMesh); meshtest.updateBound(); btRigidBody groundbody = new btRigidBody(0, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 256f / 2f))), obj, new btVector3(0, 0, 0)); btRigidBody capbody = new btRigidBody(200, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 25))), cap, new btVector3(0, 0, 0)); btRigidBody meshbody = new btRigidBody(200, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 29))), meshtest, new btVector3(0, 0, 0)); btRigidBodyConstructionInfo constructioninfotest = new btRigidBodyConstructionInfo(); constructioninfotest.m_collisionShape = new btBoxShape(new btVector3(0.5f, 0.5f, 0.5f)); constructioninfotest.m_localInertia = new btVector3(0, 0, 0); constructioninfotest.m_motionState = new btDefaultMotionState(new btTransform(new btQuaternion(0.3f, -0.4f, 0.8f, 0.1f), new btVector3(128.5f, 128, 25)), new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0.25f, 0))); constructioninfotest.m_startWorldTransform = new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0, 0)); constructioninfotest.m_mass = 2000000; constructioninfotest.m_linearDamping = 0; constructioninfotest.m_angularDamping = 0; constructioninfotest.m_friction = 0.1f; constructioninfotest.m_restitution = 0; constructioninfotest.m_linearSleepingThreshold = 0.8f; constructioninfotest.m_angularSleepingThreshold = 1; constructioninfotest.m_additionalDamping = false; constructioninfotest.m_additionalDampingFactor = 0.005f; constructioninfotest.m_additionalLinearDampingThresholdSqr = 0.01f; constructioninfotest.m_additionalAngularDampingThresholdSqr = 0.01f; constructioninfotest.m_additionalAngularDampingFactor = 0.01f; constructioninfotest.commit(); btGImpactCollisionAlgorithm.registerAlgorithm(coldisp); btRigidBody cubetest = new btRigidBody(constructioninfotest); dynamicsWorld.addRigidBody(groundbody); dynamicsWorld.addRigidBody(cubetest); dynamicsWorld.addRigidBody(capbody); dynamicsWorld.addRigidBody(meshbody); int frame = 0; for (int i = 0; i < 26; i++) { int frames = dynamicsWorld.stepSimulation(((i % 60) / 60f), 10, (1f / 60)); frame += frames; Console.WriteLine(String.Format("Cube: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, cubetest.getInterpolationWorldTransform().getOrigin().testStr(), cubetest.getWorldTransform().getRotation().testStr())); Console.WriteLine(String.Format("Cap: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, capbody.getInterpolationWorldTransform().getOrigin().testStr(), capbody.getWorldTransform().getRotation().testStr())); Console.WriteLine(String.Format("Mesh: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, meshbody.getInterpolationWorldTransform().getOrigin().testStr(), meshbody.getWorldTransform().getRotation().testStr())); } dynamicsWorld.removeRigidBody(meshbody); dynamicsWorld.removeRigidBody(capbody); dynamicsWorld.removeRigidBody(cubetest); dynamicsWorld.removeRigidBody(groundbody); cubetest.Dispose(); groundbody.Dispose(); capbody.Dispose(); cap.Dispose(); obj.Dispose(); testbtAxisSweep3.Dispose(); dynamicsWorld.Dispose(); coldisp.Dispose(); colconfig.Dispose(); seqimpconssol.Dispose(); testvec.Dispose(); testvec2.Dispose(); testvec3.Dispose(); testvec4.Dispose(); }
/* Internals */ internal void getsupport( ref btVector3 d, sSV sv ) { d.normalized( out sv.d ); //sv.d = d / d.length(); m_shape.Support( ref sv.d, out sv.w ); //sv.w = ; }
int4 FindSimplex( btVector3[] verts, int verts_count, int[] allow) { btVector3[] basis = new btVector3[3]; basis[0] = new btVector3( 0.01, 0.02, 1.0 ); int p0 = maxdirsterid( verts, verts_count, ref basis[0], allow ); btVector3 tmp; basis[0].Invert( out tmp ); int p1 = maxdirsterid( verts, verts_count, ref tmp, allow ); verts[p0].Sub( ref verts[p1], out basis[0] ); //basis[0] = verts[p0] - verts[p1]; if( p0 == p1 || basis[0].isZero() ) return new int4( -1, -1, -1, -1 ); tmp = new btVector3( (double)( 1 ), 0.02, (double)( 0 ) ); btVector3.btCross( ref tmp, ref basis[0], out basis[1] ); tmp = new btVector3( (double)( -0.02 ), (double)( 1 ), (double)( 0 ) ); btVector3.btCross( ref tmp, ref basis[0], out basis[2] ); if( basis[1].length() > basis[2].length() ) { basis[1].normalize(); } else { basis[1] = basis[2]; basis[1].normalize(); } int p2 = maxdirsterid( verts, verts_count, ref basis[1], allow ); if( p2 == p0 || p2 == p1 ) { basis[1].Invert( out tmp ); p2 = maxdirsterid( verts, verts_count, ref tmp, allow ); } if( p2 == p0 || p2 == p1 ) return new int4( -1, -1, -1, -1 ); verts[p2].Sub( ref verts[p0], out basis[1] ); btVector3.btCross( ref basis[1], ref basis[0], out tmp ); tmp.normalized( out basis[2] ); int p3 = maxdirsterid( verts, verts_count, ref basis[2], allow ); basis[2].Invert( out tmp ); if( p3 == p0 || p3 == p1 || p3 == p2 ) p3 = maxdirsterid( verts, verts_count, ref tmp, allow ); if( p3 == p0 || p3 == p1 || p3 == p2 ) return new int4( -1, -1, -1, -1 ); Debug.Assert( !( p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3 ) ); btVector3 tmp2; verts[p1].Sub( ref verts[p0], out tmp ); verts[p2].Sub( ref verts[p0], out tmp2 ); btVector3 tmp3; btVector3.btCross( ref tmp, ref tmp2, out tmp3 ); verts[p3].Sub( ref verts[p0], out tmp ); if( btVector3.btDot( ref tmp, ref tmp3 ) < 0 ) { btScalar.btSwap( ref p2, ref p3 ); } return new int4( p0, p1, p2, p3 ); }