public override void InitializeDemo()
{
// std::cerr << "initializing...\n";
m_nearClip = 1f;
m_farClip = 1000f;
m_aspect = m_glutScreenWidth / m_glutScreenHeight;
m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip);
// set up basic state
m_upAxis = 1; // start with Y-axis as "up"
m_type = PHY_ScalarType.PHY_FLOAT;
m_model = eTerrainModel.eRadial; //eFractal;
m_isDynamic = true;
// set up the physics world
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
//m_broadphase = new AxisSweep3Internal(ref worldMin,ref worldMax);
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
// initialize axis- or type-dependent physics from here
ClientResetScene();
}
public override void InitializeDemo()
{
//string filename = @"E:\users\man\bullet\xna-concaveray-output-1.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
m_cameraDistance = 400f;
m_farClip = 1500f;
m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip);
m_animatedMesh = true;
base.InitializeDemo();
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
gVertices = new ObjectArray <IndexedVector3>(totalVerts);
int indicesTotal = totalTriangles * 3;
gIndices = new ObjectArray <int>(indicesTotal);
BulletGlobals.gContactAddedCallback = null;
SetVertexPositions(waveheight, 0f);
int vertStride = 1;
int indexStride = 3;
int index = 0;
//for (int i=0;i<NUM_VERTS_X-1;i++)
//{
// for (int j=0;j<NUM_VERTS_Y-1;j++)
// {
// gIndices[index++] = j * NUM_VERTS_X + i;
// gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
// gIndices[index++] = j * NUM_VERTS_X + i + 1;
// gIndices[index++] = j * NUM_VERTS_X + i;
// gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
// gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
// }
//}
for (int i = 0; i < NUM_VERTS_X - 1; i++)
{
for (int j = 0; j < NUM_VERTS_Y - 1; j++)
{
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
}
}
TriangleIndexVertexArray indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices, indexStride, totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
float minX = NUM_VERTS_X * TRIANGLE_SIZE * 0.5f;
float minZ = NUM_VERTS_Y * TRIANGLE_SIZE * 0.5f;
//OptimizedBvh bvh = new OptimizedBvh();
IndexedVector3 aabbMin = new IndexedVector3(-minX, -5, -minZ);
IndexedVector3 aabbMax = new IndexedVector3(minX, 5, minZ);
m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
//m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression,true);
IndexedVector3 scaling = IndexedVector3.One;
CollisionShape groundShape = m_trimeshShape;
//groundShape = new TriangleMeshShape(indexVertexArrays);
//groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
IndexedVector3 up = new IndexedVector3(0.4f, 1, 0);
up.Normalize();
//groundShape = new StaticPlaneShape(up, 0f);
//groundShape = new BoxShape(new IndexedVector3(1000, 10, 1000));
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = aabbMin;
IndexedVector3 worldMax = aabbMax;
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
float mass = 0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2, -2, 0));
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, ref startTransform, groundShape);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT); //STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//m_raycastBar = new btRaycastBar (m_debugDraw,4000.0f, 0.0f,-1000,30);
m_raycastBar = new btRaycastBar(m_debugDraw, 300.0f, 0.0f, -1000, 200, worldMin.X, worldMax.X);
m_raycastBar.min_x = -100;
m_raycastBar.max_x = -100;
ClientResetScene();
}
public static void runTests()
{
String filename = @"e:\users\man\bullet\xna-math-debug-output.txt";
FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.None);
StreamWriter streamWriter = new StreamWriter(filestream);
streamWriter.WriteLine("Identity");
IndexedMatrix m = IndexedMatrix.Identity;
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,0,0");
m._basis.SetEulerZYX(2, 0, 0);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 0,2,0");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0, 2, 0);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 0,0,2");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0, 0, 2);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,2,0");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 2, 0);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,0,2");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 0, 2);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,2,2");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 2, 2);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 0.5*PI,0,0 Trans 100,100,100 MULTIPLIED BY setEuler 0,0,0.5*PI Trans -10,10,0");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(MathUtil.SIMD_HALF_PI, 0, 0);
m._origin = new IndexedVector3(100, 100, 100);
IndexedMatrix m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
m2._origin = new IndexedVector3(-10, 10, 0);
IndexedMatrix m3 = m * m2;
MathUtil.PrintMatrix(streamWriter, m3);
//streamWriter.WriteLine("Broken axis comparison");
//m = IndexedMatrix.Identity;
//m.Right = new IndexedVector3(1, 0, 0);
//m.Up = new IndexedVector3(0, 1, 0);
//m.Backward = new IndexedVector3(0, 0, 1);
//m._origin = new IndexedVector3(0, 20, 0);
//m2 = IndexedMatrix.Identity;
//m2.Right = new IndexedVector3(0, -1, 0);
//m2.Up = new IndexedVector3(1, 0, 0);
//m2.Backward = new IndexedVector3(0, 0, 1);
//m2._origin = new IndexedVector3(1, -1, -1);
//m3 = m * m2;
//MathUtil.PrintMatrix(streamWriter, m3);
//streamWriter.WriteLine("setEuler 0.5*PI,0,0 Trans 0,0,0 MULTIPLIED BY setEuler 0,0,0.5*PI Trans -10,10,0");
//m = IndexedMatrix.Identity;
//m._basis.SetEulerZYX(MathUtil.SIMD_HALF_PI, 0, 0);
//m._origin = new IndexedVector3(0, 0, 0);
//m2 = IndexedMatrix.Identity;
//m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
//m2._origin = new IndexedVector3(-10, 10, 0);
//m3 = m * m2;
//MathUtil.PrintMatrix(streamWriter, m3);
//streamWriter.WriteLine("setEuler 0.25*PI,0,0 Trans 33,0,0 MULTIPLIED BY setEuler 0,0,0.5*PI Trans 0,0,0");
//m = IndexedMatrix.Identity;
//m._basis.SetEulerZYX(MathUtil.SIMD_QUARTER_PI, 0, 0);
//m._origin = new IndexedVector3(33, 0, 0);
//m2 = IndexedMatrix.Identity;
//m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
//m2._origin = new IndexedVector3(0, 0, 0);
//m3 = m *m2;
//MathUtil.PrintMatrix(streamWriter, m3);
streamWriter.WriteLine("transposeTimes");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 1, 2);
m._origin = new IndexedVector3(3, 3, 3);
streamWriter.WriteLine("");
//MathUtil.PrintMatrix(streamWriter, m);
m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(1, 2, -2);
m2._origin = new IndexedVector3(5, 2, 13);
//MathUtil.PrintMatrix(streamWriter, m2);
IndexedBasisMatrix im3 = m._basis.TransposeTimes(m2._basis);
MathUtil.PrintMatrix(streamWriter, im3);
streamWriter.WriteLine("inverseTransform.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
IndexedVector3 v = new IndexedVector3(20, 25, 30);
IndexedVector3 result = MathUtil.InverseTransform(ref m, ref v);
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("inverseTimes.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(0.3f, 0.8f, 2.3f);
m2._origin = new IndexedVector3(20, 25, 30);
m3 = m.InverseTimes(ref m2);
MathUtil.PrintMatrix(streamWriter, m3);
streamWriter.WriteLine("Transform.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
v = new IndexedVector3(20, 25, 30);
result = m * v;
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("TransformNormal.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
v = new IndexedVector3(20, 25, 30);
//IndexedMatrix tm = IndexedMatrix.Transpose(m);
//result = IndexedVector3.TransformNormal(v,tm);
result = m._basis * v;
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("quatAngle");
Quaternion q = Quaternion.CreateFromYawPitchRoll(0.3f, 0.2f, 0.7f);
float fresult = MathUtil.QuatAngle(ref q);
streamWriter.WriteLine(String.Format("{0:0.00000000}", fresult));
streamWriter.WriteLine("quatRotate");
v = new IndexedVector3(20, -25, 30);
result = MathUtil.QuatRotate(q, v);
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("shortestArcQuat");
v = new IndexedVector3(2f, -1f, 3f);
IndexedVector3 v2 = new IndexedVector3(0.5f, 0.1f, 0.7f);
q = MathUtil.ShortestArcQuat(v, v2);
MathUtil.PrintQuaternion(streamWriter, q);
streamWriter.WriteLine("");
streamWriter.WriteLine("quaternionMultiply");
q = Quaternion.CreateFromYawPitchRoll(0.3f, 0.2f, 0.7f);
Quaternion q2 = Quaternion.CreateFromYawPitchRoll(-0.71f, 0.8f, 0.3f);
q = MathUtil.QuaternionMultiply(q, q2);
MathUtil.PrintQuaternion(streamWriter, q);
streamWriter.WriteLine("");
//streamWriter.WriteLine("matrixToEulerXYZ");
//m = IndexedMatrix.Identity;
//m._basis..SetEulerZYX(1, -2, -1);
//m._origin = new IndexedVector3(-1, 2, -3);
//MathUtil.MatrixToEulerXYZ(ref m, out result);
//MathUtil.PrintVector3(streamWriter, result);
//streamWriter.WriteLine("");
streamWriter.WriteLine("getSkewSymmetrixMatrix");
v = new IndexedVector3(0.2f, 0.7f, -0.3f);
IndexedVector3 v3;
IndexedVector3 v4;;
MathUtil.GetSkewSymmetricMatrix(ref v, out v2, out v3, out v4);
MathUtil.PrintVector3(streamWriter, v2);
streamWriter.WriteLine("");
MathUtil.PrintVector3(streamWriter, v3);
streamWriter.WriteLine("");
MathUtil.PrintVector3(streamWriter, v4);
streamWriter.WriteLine("");
streamWriter.WriteLine("quaternion create");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
m._origin = new IndexedVector3(0.0f, 0.30f, 0.0f);
m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
m2._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
q = m.GetRotation();
q2 = m2.GetRotation();
MathUtil.PrintQuaternion(streamWriter, q);
streamWriter.WriteLine("");
MathUtil.PrintQuaternion(streamWriter, q2);
streamWriter.WriteLine("");
streamWriter.WriteLine("row and column");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0.3f, 0.7f, 0.5f);
m._origin = new IndexedVector3(11, 22, 33);
MathUtil.PrintVector3(streamWriter, "col0", m._basis.GetColumn(0));
MathUtil.PrintVector3(streamWriter, "col1", m._basis.GetColumn(1));
MathUtil.PrintVector3(streamWriter, "col2", m._basis.GetColumn(2));
//MathUtil.PrintVector3(streamWriter, "col3", MathUtil.MatrixColumn(m, 0));
MathUtil.PrintVector3(streamWriter, "row0", m._basis.GetRow(0));
MathUtil.PrintVector3(streamWriter, "row1", m._basis.GetRow(1));
MathUtil.PrintVector3(streamWriter, "row2", m._basis.GetRow(2));
//MathUtil.PrintVector3(streamWriter, "row3", MathUtil.MatrixRow(ref m, 3));
Matrix lookat1 = Matrix.CreateLookAt(new Vector3(5, 5, 5), new Vector3(10, 10, 10), new Vector3(0, 1, 0));
IndexedMatrix lookat2 = IndexedMatrix.CreateLookAt(new IndexedVector3(5, 5, 5), new IndexedVector3(10, 10, 10), new IndexedVector3(0, 1, 0));
Matrix compare = lookat2.ToMatrix();
MathUtil.PrintMatrix(streamWriter, "lookat1", lookat1);
MathUtil.PrintMatrix(streamWriter, "lookat2", lookat2);
MathUtil.PrintMatrix(streamWriter, "lookat compare", compare);
float aspect = (float)(800.0f / 600.0f);
float fov = MathHelper.ToRadians(40.0f);
float near = 1f;
float far = 500f;
Matrix pov = Matrix.CreatePerspectiveFieldOfView(fov, aspect, near, far);
IndexedMatrix pov2 = IndexedMatrix.CreatePerspectiveFieldOfView(fov, aspect, near, far);
Matrix pov3 = pov2.ToMatrix();
MathUtil.PrintMatrix(streamWriter, "pov1", pov);
MathUtil.PrintMatrix(streamWriter, "pov2", pov2);
MathUtil.PrintMatrix(streamWriter, "pov compare", pov3);
streamWriter.WriteLine("Complete.");
streamWriter.Flush();
filestream.Close();
}
