public HeightfieldTerrainShape(int heightStickWidth, int heightStickLength,
IntPtr heightfieldData, float heightScale, float minHeight, float maxHeight,
int upAxis, PhyScalarType heightDataType, bool flipQuadEdges)
: base(btHeightfieldTerrainShape_new(heightStickWidth, heightStickLength,
heightfieldData, heightScale, minHeight, maxHeight, upAxis, heightDataType,
flipQuadEdges))
{
}
public void GetLockedMaterialBase(out IntPtr materialBase, out int numMaterials,
out PhyScalarType materialType, out int materialStride, out IntPtr triangleMaterialBase,
out int numTriangles, out int triangleMaterialStride, out PhyScalarType triangleType)
{
btTriangleIndexVertexMaterialArray_getLockedMaterialBase(_native, out materialBase,
out numMaterials, out materialType, out materialStride, out triangleMaterialBase,
out numTriangles, out triangleMaterialStride, out triangleType);
}
public void GetLockedVertexIndexBase(out IntPtr vertexBase, out int numVerts,
out PhyScalarType type, out int vertexStride, out IntPtr indexbase, out int indexStride,
out int numFaces, out PhyScalarType indicesType, int subpart)
{
btStridingMeshInterface_getLockedVertexIndexBase2(_native, out vertexBase,
out numVerts, out type, out vertexStride, out indexbase, out indexStride,
out numFaces, out indicesType, subpart);
}
public HeightfieldTerrainShape(int heightStickWidth, int heightStickLength,
IntPtr heightfieldData, float heightScale, float minHeight, float maxHeight,
int upAxis, PhyScalarType heightDataType, bool flipQuadEdges)
: base(btHeightfieldTerrainShape_new(heightStickWidth, heightStickLength,
heightfieldData, heightScale, minHeight, maxHeight, upAxis, heightDataType,
flipQuadEdges))
{
}
public void GetLockedVertexIndexBase(out IntPtr vertexBase, out int numVerts,
out PhyScalarType type, out int vertexStride, out IntPtr indexbase, out int indexStride,
out int numFaces, out PhyScalarType indicesType, int subpart)
{
btStridingMeshInterface_getLockedVertexIndexBase2(_native, out vertexBase,
out numVerts, out type, out vertexStride, out indexbase, out indexStride,
out numFaces, out indicesType, subpart);
}
private static float ReadFloat(BinaryReader vertexReader, PhyScalarType vertexType)
{
if (vertexType == PhyScalarType.Double)
{
return((float)vertexReader.ReadDouble());
}
return(vertexReader.ReadSingle());
}
public HeightfieldTerrainShape(int heightStickWidth, int heightStickLength,
IntPtr heightfieldData, float heightScale, float minHeight, float maxHeight,
int upAxis, PhyScalarType heightDataType, bool flipQuadEdges)
{
IntPtr native = btHeightfieldTerrainShape_new(heightStickWidth, heightStickLength,
heightfieldData, heightScale, minHeight, maxHeight, upAxis, heightDataType,
flipQuadEdges);
InitializeCollisionShape(native);
}
private void CreateHeightfieldTerrain()
{
const float minHeight = 0;
const float maxHeight = 10.0f;
const float heightScale = maxHeight / 256.0f;
const int width = 64, length = 64;
const int dataLength = width * length * sizeof(float);
const PhyScalarType scalarType = PhyScalarType.Single;
var scale = new Vector3(15.0f, maxHeight, 15.0f);
_terrainData = Marshal.AllocHGlobal(dataLength);
var terrain = new byte[dataLength];
using (var file = new MemoryStream(terrain))
{
using (var writer = new BinaryWriter(file))
{
for (int i = 0; i < width; i++)
{
for (int j = 0; j < length; j++)
{
writer.Write((float)((maxHeight / 2) + 4 * Math.Sin(j * 0.5f) * Math.Cos(i)));
}
}
}
}
Marshal.Copy(terrain, 0, _terrainData, terrain.Length);
var groundShape = new HeightfieldTerrainShape(width, length,
_terrainData, heightScale, minHeight, maxHeight, upIndex, scalarType, false);
groundShape.SetUseDiamondSubdivision(true);
groundShape.LocalScaling = new Vector3(scale.X, 1, scale.Z);
Matrix transform = Matrix.Translation(-scale.X / 2, scale.Y / 2, -scale.Z / 2);
RigidBody ground = PhysicsHelper.CreateStaticBody(transform, groundShape, World);
ground.UserObject = "Ground";
Matrix vehicleTransform = Matrix.Translation(new Vector3(20, 3, -3));
CreateVehicle(vehicleTransform);
}
private void CreateHeightfieldTerrainFromFile()
{
const float minHeight = 0;
const float maxHeight = 10.0f;
const float heightScale = maxHeight / 256.0f;
const int width = 128, length = 128;
const int dataLength = width * length * sizeof(byte);
const PhyScalarType scalarType = PhyScalarType.Byte;
var scale = new Vector3(5.0f, maxHeight, 5.0f);
string heightfieldFile = Path.Combine("data", "heightfield128x128.raw");
_terrainData = Marshal.AllocHGlobal(dataLength);
using (var stream = new FileStream(heightfieldFile, FileMode.Open, FileAccess.Read))
{
using (var reader = new BinaryReader(stream))
{
while (stream.Position < stream.Length)
{
int offset = (int)stream.Position;
byte height = reader.ReadByte();
Marshal.WriteByte(_terrainData, offset, height);
}
}
}
var shape = new HeightfieldTerrainShape(width, length,
_terrainData, heightScale, minHeight, maxHeight, upIndex, scalarType, false);
shape.SetUseDiamondSubdivision(true);
shape.LocalScaling = new Vector3(scale.X, 1, scale.Z);
Matrix transform = Matrix.Translation(-scale.X / 2, scale.Y / 2, -scale.Z / 2);
RigidBody ground = PhysicsHelper.CreateStaticBody(transform, shape, World);
ground.UserObject = "Ground";
Matrix vehicleTransform = Matrix.Translation(new Vector3(20, 3, -3));
CreateVehicle(vehicleTransform);
}
public void AddMaterialProperties(MaterialProperties mat, PhyScalarType triangleType = PhyScalarType.Int32)
{
btTriangleIndexVertexMaterialArray_addMaterialProperties(_native, mat._native,
triangleType);
}
static extern void btMaterialProperties_setTriangleType(IntPtr obj, PhyScalarType value);
static extern void btGImpactMeshShapePart_TrimeshPrimitiveManager_setType(IntPtr obj, PhyScalarType value);
static extern void btTriangleIndexVertexMaterialArray_getLockedMaterialBase(IntPtr obj, [Out] out IntPtr materialBase, [Out] out int numMaterials, [Out] out PhyScalarType materialType, [Out] out int materialStride, [Out] out IntPtr triangleMaterialBase, [Out] out int numTriangles, [Out] out int triangleMaterialStride, [Out] out PhyScalarType triangleType);
static extern void btTriangleIndexVertexMaterialArray_addMaterialProperties2(IntPtr obj, IntPtr mat, PhyScalarType triangleType);
static extern void btTriangleIndexVertexMaterialArray_getLockedReadOnlyMaterialBase2(IntPtr obj, out IntPtr materialBase, out int numMaterials, out PhyScalarType materialType, out int materialStride, out IntPtr triangleMaterialBase, out int numTriangles, out int triangleMaterialStride, out PhyScalarType triangleType, int subpart);
public Physics(VehicleDemo game)
{
CollisionShape groundShape = new BoxShape(50, 3, 50);
CollisionShapes.Add(groundShape);
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Solver = new SequentialImpulseConstraintSolver();
Vector3 worldMin = new Vector3(-10000, -10000, -10000);
Vector3 worldMax = new Vector3(10000, 10000, 10000);
Broadphase = new AxisSweep3(worldMin, worldMax);
//Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
int i;
Matrix tr;
Matrix vehicleTr;
if (UseTrimeshGround)
{
const float scale = 20.0f;
//create a triangle-mesh ground
int vertStride = Vector3.SizeInBytes;
int indexStride = 3 * sizeof(int);
const int NUM_VERTS_X = 20;
const int NUM_VERTS_Y = 20;
const int totalVerts = NUM_VERTS_X * NUM_VERTS_Y;
const int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.Allocate(totalVerts, vertStride, totalTriangles, indexStride, PhyScalarType.Int32, PhyScalarType.Single);
BulletSharp.DataStream data = mesh.LockVerts();
for (i = 0; i < NUM_VERTS_X; i++)
{
for (int j = 0; j < NUM_VERTS_Y; j++)
{
float wl = .2f;
float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));
data.Write((i - NUM_VERTS_X * 0.5f) * scale);
data.Write(height);
data.Write((j - NUM_VERTS_Y * 0.5f) * scale);
}
}
int index = 0;
IntArray idata = mesh.TriangleIndices;
for (i = 0; i < NUM_VERTS_X - 1; i++)
{
for (int j = 0; j < NUM_VERTS_Y - 1; j++)
{
idata[index++] = j * NUM_VERTS_X + i;
idata[index++] = j * NUM_VERTS_X + i + 1;
idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;
idata[index++] = j * NUM_VERTS_X + i;
idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;
idata[index++] = (j + 1) * NUM_VERTS_X + i;
}
}
vertexArray.AddIndexedMesh(mesh);
groundShape = new BvhTriangleMeshShape(vertexArray, true);
tr = Matrix.Identity;
vehicleTr = Matrix.Translation(0, -2, 0);
}
else
{
// Use HeightfieldTerrainShape
int width = 40, length = 40;
//int width = 128, length = 128; // Debugging is too slow for this
float maxHeight = 10.0f;
float heightScale = maxHeight / 256.0f;
Vector3 scale = new Vector3(20.0f, maxHeight, 20.0f);
//PhyScalarType scalarType = PhyScalarType.PhyUChar;
//FileStream file = new FileStream(heightfieldFile, FileMode.Open, FileAccess.Read);
// Use float data
PhyScalarType scalarType = PhyScalarType.Single;
byte[] terr = new byte[width * length * 4];
MemoryStream file = new MemoryStream(terr);
BinaryWriter writer = new BinaryWriter(file);
for (i = 0; i < width; i++)
{
for (int j = 0; j < length; j++)
{
writer.Write((float)((maxHeight / 2) + 4 * Math.Sin(j * 0.5f) * Math.Cos(i)));
}
}
writer.Flush();
file.Position = 0;
HeightfieldTerrainShape heightterrainShape = new HeightfieldTerrainShape(width, length,
file, heightScale, 0, maxHeight, upIndex, scalarType, false);
heightterrainShape.SetUseDiamondSubdivision(true);
groundShape = heightterrainShape;
groundShape.LocalScaling = new Vector3(scale.X, 1, scale.Z);
tr = Matrix.Translation(new Vector3(-scale.X / 2, scale.Y / 2, -scale.Z / 2));
vehicleTr = Matrix.Translation(new Vector3(20, 3, -3));
// Create graphics object
file.Position = 0;
BinaryReader reader = new BinaryReader(file);
int totalTriangles = (width - 1) * (length - 1) * 2;
int totalVerts = width * length;
game.groundMesh = new Mesh(game.Device, totalTriangles, totalVerts,
MeshFlags.SystemMemory | MeshFlags.Use32Bit, VertexFormat.Position | VertexFormat.Normal);
SlimDX.DataStream data = game.groundMesh.LockVertexBuffer(LockFlags.None);
for (i = 0; i < width; i++)
{
for (int j = 0; j < length; j++)
{
float height;
if (scalarType == PhyScalarType.Single)
{
// heightScale isn't applied internally for float data
height = reader.ReadSingle();
}
else if (scalarType == PhyScalarType.Byte)
{
height = file.ReadByte() * heightScale;
}
else
{
height = 0.0f;
}
data.Write((j - length * 0.5f) * scale.X);
data.Write(height);
data.Write((i - width * 0.5f) * scale.Z);
// Normals will be calculated later
data.Position += 12;
}
}
game.groundMesh.UnlockVertexBuffer();
file.Close();
data = game.groundMesh.LockIndexBuffer(LockFlags.None);
for (i = 0; i < width - 1; i++)
{
for (int j = 0; j < length - 1; j++)
{
// Using diamond subdivision
if ((j + i) % 2 == 0)
{
data.Write(j * width + i);
data.Write((j + 1) * width + i + 1);
data.Write(j * width + i + 1);
data.Write(j * width + i);
data.Write((j + 1) * width + i);
data.Write((j + 1) * width + i + 1);
}
else
{
data.Write(j * width + i);
data.Write((j + 1) * width + i);
data.Write(j * width + i + 1);
data.Write(j * width + i + 1);
data.Write((j + 1) * width + i);
data.Write((j + 1) * width + i + 1);
}
/*
* // Not using diamond subdivision
* data.Write(j * width + i);
* data.Write((j + 1) * width + i);
* data.Write(j * width + i + 1);
*
* data.Write(j * width + i + 1);
* data.Write((j + 1) * width + i);
* data.Write((j + 1) * width + i + 1);
*/
}
}
game.groundMesh.UnlockIndexBuffer();
game.groundMesh.ComputeNormals();
}
CollisionShapes.Add(groundShape);
//create ground object
RigidBody ground = LocalCreateRigidBody(0, tr, groundShape);
ground.UserObject = "Ground";
CollisionShape chassisShape = new BoxShape(1.0f, 0.5f, 2.0f);
CollisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
CollisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
Matrix localTrans = Matrix.Translation(Vector3.UnitY);
compound.AddChildShape(localTrans, chassisShape);
RigidBody carChassis = LocalCreateRigidBody(800, Matrix.Identity, compound);
carChassis.UserObject = "Chassis";
//carChassis.SetDamping(0.2f, 0.2f);
//CylinderShapeX wheelShape = new CylinderShapeX(wheelWidth, wheelRadius, wheelRadius);
// clientResetScene();
// create vehicle
RaycastVehicle.VehicleTuning tuning = new RaycastVehicle.VehicleTuning();
IVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(World);
vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
carChassis.ActivationState = ActivationState.DisableDeactivation;
World.AddAction(vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel = true;
// choose coordinate system
vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
Vector3 connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
WheelInfo a = vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
isFrontWheel = false;
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
for (i = 0; i < vehicle.NumWheels; i++)
{
WheelInfo wheel = vehicle.GetWheelInfo(i);
wheel.SuspensionStiffness = suspensionStiffness;
wheel.WheelsDampingRelaxation = suspensionDamping;
wheel.WheelsDampingCompression = suspensionCompression;
wheel.FrictionSlip = wheelFriction;
wheel.RollInfluence = rollInfluence;
}
vehicle.RigidBody.WorldTransform = vehicleTr;
}
static extern void btGImpactMeshShapePart_TrimeshPrimitiveManager_setType(IntPtr obj, PhyScalarType value);
static extern void btIndexedMesh_setVertexType(IntPtr obj, PhyScalarType value);
static extern void btTriangleIndexVertexMaterialArray_addMaterialProperties(IntPtr obj, IntPtr mat, PhyScalarType triangleType);
private static Vector3[] CreateTriangleMesh(StridingMeshInterface meshInterface)
{
// StridingMeshInterface can only be TriangleIndexVertexArray
var meshes = (meshInterface as TriangleIndexVertexArray).IndexedMeshArray;
int numTriangles = 0;
foreach (var mesh in meshes)
{
numTriangles += mesh.NumTriangles;
}
int numVertices = numTriangles * 3;
Vector3[] vertices = new Vector3[numVertices * 2];
int v = 0;
for (int part = 0; part < meshInterface.NumSubParts; part++)
{
var mesh = meshes[part];
var indexStream = mesh.GetTriangleStream();
var vertexStream = mesh.GetVertexStream();
var indexReader = new BinaryReader(indexStream);
var vertexReader = new BinaryReader(vertexStream);
int vertexStride = mesh.VertexStride;
int triangleStrideDelta = mesh.TriangleIndexStride - 3 * sizeof(int);
PhyScalarType vertexType = mesh.VertexType;
while (indexStream.Position < indexStream.Length)
{
uint i = indexReader.ReadUInt32();
vertexStream.Position = vertexStride * i;
float f1 = ReadFloat(vertexReader, vertexType);
float f2 = ReadFloat(vertexReader, vertexType);
float f3 = ReadFloat(vertexReader, vertexType);
Vector3 v0 = new Vector3(f1, f2, f3);
i = indexReader.ReadUInt32();
vertexStream.Position = vertexStride * i;
f1 = vertexReader.ReadSingle();
f2 = vertexReader.ReadSingle();
f3 = vertexReader.ReadSingle();
Vector3 v1 = new Vector3(f1, f2, f3);
i = indexReader.ReadUInt32();
vertexStream.Position = vertexStride * i;
f1 = ReadFloat(vertexReader, vertexType);
f2 = ReadFloat(vertexReader, vertexType);
f3 = ReadFloat(vertexReader, vertexType);
Vector3 v2 = new Vector3(f1, f2, f3);
Vector3 v01 = v0 - v1;
Vector3 v02 = v0 - v2;
Vector3 normal = Vector3.Cross(v01, v02);
normal.Normalize();
var scaling = meshInterface.Scaling;
vertices[v++] = v0 * scaling;
vertices[v++] = normal * scaling;
vertices[v++] = v1 * scaling;
vertices[v++] = normal * scaling;
vertices[v++] = v2 * scaling;
vertices[v++] = normal * scaling;
indexStream.Position += triangleStrideDelta;
}
indexStream.Dispose();
vertexStream.Dispose();
}
return(vertices);
}
public void Allocate(int numTriangles, int numVertices, int triangleIndexStride = sizeof(int) * 3, int vertexStride = sizeof(float) * 3)
{
if (_ownsData)
{
Free();
}
else
{
_ownsData = true;
}
switch (triangleIndexStride)
{
case sizeof(byte) * 3:
IndexType = PhyScalarType.UChar;
break;
case sizeof(short) * 3:
IndexType = PhyScalarType.Short;
break;
case sizeof(int) * 3:
default:
IndexType = PhyScalarType.Integer;
break;
}
VertexType = PhyScalarType.Float;
NumTriangles = numTriangles;
TriangleIndexBase = Marshal.AllocHGlobal(numTriangles * triangleIndexStride);
TriangleIndexStride = triangleIndexStride;
NumVertices = numVertices;
VertexBase = Marshal.AllocHGlobal(numVertices * vertexStride);
VertexStride = vertexStride;
}
static extern IntPtr btHeightfieldTerrainShape_new(int heightStickWidth, int heightStickLength, IntPtr heightfieldData, float heightScale, float minHeight, float maxHeight, int upAxis, PhyScalarType heightDataType, bool flipQuadEdges);
static extern void btStridingMeshInterface_getLockedVertexIndexBase(IntPtr obj, out IntPtr vertexbase, out int numverts, out PhyScalarType type, out int vertexStride, out IntPtr indexbase, out int indexstride, out int numfaces, out PhyScalarType indicestype);
static extern void btTriangleIndexVertexMaterialArray_getLockedReadOnlyMaterialBase(IntPtr obj, out IntPtr materialBase, out int numMaterials, out PhyScalarType materialType, out int materialStride, out IntPtr triangleMaterialBase, out int numTriangles, out int triangleMaterialStride, out PhyScalarType triangleType, int subpart);
static extern void btTriangleIndexVertexArray_addIndexedMesh(IntPtr obj, IntPtr mesh, PhyScalarType indexType);
static extern IntPtr btHeightfieldTerrainShape_new(int heightStickWidth, int heightStickLength, IntPtr heightfieldData, float heightScale, float minHeight, float maxHeight, int upAxis, PhyScalarType heightDataType, bool flipQuadEdges);
public void AddIndexedMesh(IndexedMesh mesh, PhyScalarType indexType)
{
_meshes.Add(mesh);
btTriangleIndexVertexArray_addIndexedMesh2(_native, mesh._native, indexType);
}
public void AddIndexedMesh(IndexedMesh mesh, PhyScalarType indexType = PhyScalarType.Int32)
{
_meshes.Add(mesh);
btTriangleIndexVertexArray_addIndexedMesh(_native, mesh._native, indexType);
}
static extern void btStridingMeshInterface_getLockedVertexIndexBase2(IntPtr obj, out IntPtr vertexbase, out int numverts, out PhyScalarType type, out int vertexStride, out IntPtr indexbase, out int indexstride, out int numfaces, out PhyScalarType indicestype, int subpart);
static extern void btIndexedMesh_setVertexType(IntPtr obj, PhyScalarType value);
static extern void btMaterialProperties_setTriangleType(IntPtr obj, PhyScalarType value);
static extern void btTriangleIndexVertexArray_addIndexedMesh2(IntPtr obj, IntPtr mesh, PhyScalarType indexType);
public void AddMaterialProperties(MaterialProperties mat, PhyScalarType triangleType)
{
btTriangleIndexVertexMaterialArray_addMaterialProperties2(_native, mat._native, triangleType);
}
static extern void btStridingMeshInterface_getLockedVertexIndexBase2(IntPtr obj, [Out] out IntPtr vertexbase, [Out] out int numverts, [Out] out PhyScalarType type, [Out] out int stride, [Out] out IntPtr indexbase, [Out] out int indexstride, [Out] out int numfaces, [Out] out PhyScalarType indicestype, int subpart);
public void GetLockedReadOnlyMaterialBase(out IntPtr materialBase, out int numMaterials, out PhyScalarType materialType, out int materialStride, out IntPtr triangleMaterialBase, out int numTriangles, out int triangleMaterialStride, out PhyScalarType triangleType, int subpart)
{
btTriangleIndexVertexMaterialArray_getLockedReadOnlyMaterialBase2(_native, out materialBase, out numMaterials, out materialType, out materialStride, out triangleMaterialBase, out numTriangles, out triangleMaterialStride, out triangleType, subpart);
}
