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 );
}
