public void SetUp()
{
int[] triangles1 = new[] { 0, 1, 2 };
Vector3[] vertices1 = new Vector3[]
{
new Vector3(0, 0, 0),
new Vector3(1, 0, 0),
new Vector3(0, 1, 0)
};
_meshInterface = new TriangleIndexVertexArray(triangles1, vertices1);
int[] triangles2 = new[] { 0, 1, 2 };
Vector3[] vertices2 = new Vector3[]
{
new Vector3(0, 0, 3),
new Vector3(1, 0, 3),
new Vector3(0, 1, 3)
};
_indexedMesh = new IndexedMesh();
_indexedMesh.Allocate(triangles2.Length / 3, vertices2.Length);
_indexedMesh.SetData(triangles2, vertices2);
_meshInterface.AddIndexedMesh(_indexedMesh);
_impactMesh = new GImpactMeshShape(_meshInterface);
}
private CollisionShape LoadShapeFromFile(string fileName, float mass, Vector3 scale, string baseDirectory)
{
string fullPath = Path.Combine(baseDirectory, fileName);
string extension = Path.GetExtension(fullPath);
switch (extension)
{
case ".obj":
WavefrontObj obj = WavefrontObj.Load(fullPath);
var mesh = CreateTriangleMesh(obj.Indices, obj.Vertices, scale);
if (mass == 0)
{
const bool useQuantization = true;
return(new BvhTriangleMeshShape(mesh, useQuantization));
}
else
{
// TODO: convex decomposition
GImpactCollisionAlgorithm.RegisterAlgorithm((CollisionDispatcher)World.Dispatcher);
var shape = new GImpactMeshShape(mesh);
shape.Margin = 0;
shape.UpdateBound();
return(shape);
}
default:
throw new NotSupportedException();
}
}
void InitGImpactCollision()
{
// Create Torus Shape
indexVertexArrays = new TriangleIndexVertexArray(TorusMesh.Indices, TorusMesh.Vertices);
#if BULLET_GIMPACT
#if BULLET_GIMPACT_CONVEX_DECOMPOSITION
//GImpactConvexDecompositionShape trimesh =
// new GImpactConvexDecompositionShape(indexVertexArrays, new Vector3(1), 0.01f);
//trimesh.Margin = 0.07f;
//trimesh.UpdateBound();
#else
GImpactMeshShape trimesh = new GImpactMeshShape(indexVertexArrays);
trimesh.LocalScaling = new Vector3(1);
#if BULLET_TRIANGLE_COLLISION
trimesh.Margin = 0.07f; //?????
#else
trimesh.Margin = 0;
#endif
trimesh.UpdateBound();
#endif
trimeshShape = trimesh;
#else
//trimeshShape = new GImpactMeshData(indexVertexArrays);
#endif
/// Create Bunny Shape
indexVertexArrays2 = new TriangleIndexVertexArray(BunnyMesh.Indices, BunnyMesh.Vertices);
#if BULLET_GIMPACT
#if BULLET_GIMPACT_CONVEX_DECOMPOSITION
//GImpactConvexDecompositionShape trimesh2 =
// new GImpactConvexDecompositionShape(indexVertexArrays, new Vector3(1), 0.01f);
//trimesh.Margin = 0.07f;
//trimesh.UpdateBound();
//trimeshShape = trimesh2;
#else
GImpactMeshShape trimesh2 = new GImpactMeshShape(indexVertexArrays2);
trimesh2.LocalScaling = new Vector3(1);
#if BULLET_TRIANGLE_COLLISION
trimesh2.Margin = 0.07f; //?????
#else
trimesh2.Margin = 0;
#endif
trimesh2.UpdateBound();
trimeshShape2 = trimesh2;
#endif
#else
//trimeshShape2 = new GImpactMeshData(indexVertexArrays2);
#endif
//register GIMPACT algorithm
#if BULLET_GIMPACT
GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
#else
//ConcaveConcaveCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
#endif
}
public void SetUp()
{
conf = new DefaultCollisionConfiguration();
dispatcher = new CollisionDispatcher(conf);
broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);
// Initialize TriangleIndexVertexArray with float array
indexVertexArray = new TriangleIndexVertexArray(TorusMesh.Indices, TorusMesh.Vertices);
gImpactMeshShape = new GImpactMeshShape(indexVertexArray);
gImpactMeshShape.CalculateLocalInertia(1.0f);
gImpactMesh = CreateBody(1.0f, gImpactMeshShape, Vector3.Zero);
// Initialize TriangleIndexVertexArray with Vector3 array
Vector3[] torusVertices = new Vector3[TorusMesh.Vertices.Length / 3];
for (int i = 0; i < torusVertices.Length; i++)
{
torusVertices[i] = new Vector3(
TorusMesh.Vertices[i * 3],
TorusMesh.Vertices[i * 3 + 1],
TorusMesh.Vertices[i * 3 + 2]);
}
indexVertexArray2 = new TriangleIndexVertexArray(TorusMesh.Indices, torusVertices);
triangleMeshShape = new BvhTriangleMeshShape(indexVertexArray2, true);
// CalculateLocalInertia must fail for static shapes (shapes based on TriangleMeshShape)
//triangleMeshShape.CalculateLocalInertia(1.0f);
triangleMesh = CreateBody(0.0f, triangleMeshShape, Vector3.Zero);
}
private GImpactMeshShape CreateGImpactShape(TriangleIndexVertexArray shapeData)
{
var shape = new GImpactMeshShape(shapeData);
shape.Margin = 0;
shape.UpdateBound();
return(shape);
}
public override void Run()
{
var conf = new DefaultCollisionConfiguration();
var dispatcher = new CollisionDispatcher(conf);
var broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);
var indexVertexArray = new TriangleIndexVertexArray(TorusMesh.Indices, TorusMesh.Vertices);
foreach (var indexedMesh in indexVertexArray.IndexedMeshArray)
{
indexedMesh.ToString();
}
AddToDisposeQueue(indexVertexArray);
var gImpactMesh = new GImpactMeshShape(indexVertexArray);
Vector3 aabbMin, aabbMax;
gImpactMesh.GetAabb(Matrix.Identity, out aabbMin, out aabbMax);
CreateBody(1.0f, gImpactMesh, Vector3.Zero);
AddToDisposeQueue(gImpactMesh);
gImpactMesh = null;
var triangleMesh = new BvhTriangleMeshShape(indexVertexArray, true);
triangleMesh.CalculateLocalInertia(1.0f);
triangleMesh.GetAabb(Matrix.Identity, out aabbMin, out aabbMax);
CreateBody(1.0f, triangleMesh, Vector3.Zero);
AddToDisposeQueue(triangleMesh);
triangleMesh = null;
indexVertexArray = null;
AddToDisposeQueue(conf);
AddToDisposeQueue(dispatcher);
AddToDisposeQueue(broadphase);
AddToDisposeQueue(world);
//conf.Dispose();
conf = null;
//dispatcher.Dispose();
dispatcher = null;
//broadphase.Dispose();
broadphase = null;
for (int i = 0; i < 600; i++)
{
world.StepSimulation(1.0f / 60.0f);
}
world.Dispose();
world = null;
ForceGC();
TestWeakRefs();
ClearRefs();
}
// Loads the given OBJ file, creates a RigidBody with the given mess, and places
// at the origin of the ground.
protected void PrepareSimObj(string objFile, float mass, BulletSharp.Math.Vector3 inertia)
{
Debug.Log("Loading " + objFile + "...");
// Load wavefront file
OBJLoader.OBJMesh objloadermesh = OBJLoader.LoadOBJMesh(objFile);
Debug.Assert(objloadermesh.vertices.Count > 0);
Debug.Assert(objloadermesh.faces.Count > 0);
// Debug.Log("VERTS: " + objloadermesh.vertices.Count.ToString());
// Debug.Log("FACES: " + objloadermesh.faces.Count.ToString());
m_btmesh = DataGenUtils.BulletMeshFromUnity(objloadermesh);
Debug.Assert(m_btmesh.vertices.Length > 0);
Debug.Assert(m_btmesh.indices.Length > 0);
// Debug.Log("btVERTS: " + (btmesh.vertices.Length / 3).ToString());
// Debug.Log("btFACES: " + (btmesh.indices.Length / 3).ToString());
// Create a GImpactMeshShape for collider
var triVtxarray = new TriangleIndexVertexArray(m_btmesh.indices, m_btmesh.vertices);
m_cs = new GImpactMeshShape(triVtxarray);
m_cs.LocalScaling = new BulletSharp.Math.Vector3(1);
m_cs.Margin = bodyMargin;
m_cs.UpdateBound();
AddCollisionShape(m_cs);
// move it up so resting on the ground plane
float miny = float.MaxValue;
float cury;
for (int i = 0; i < objloadermesh.vertices.Count; i++)
{
cury = objloadermesh.vertices[i][1];
if (cury < miny)
{
miny = cury;
}
}
miny = -miny;
m_rbInitTransVec = new BulletSharp.Math.Vector3(0, miny + bodyMargin + m_groundMargin, 0);
m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); // * Matrix.RotationY(Random.Range(0.0f, 360.0f));
m_rb = CreateRigidBody(mass, inertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction, viz: RENDER_MODE);
m_rb.AngularFactor = angularFactor;
m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
if (DEBUG)
{
Debug.Log("LOADED MOMENT: " + m_rb.LocalInertia.ToString());
}
// if (DEBUG) Debug.Log("WORLD MOMENT: " + m_rb.InvInertiaTensorWorld.ToString());
// Debug.Log("Min y: " + (-miny).ToString());
if (DEBUG)
{
Debug.Log(m_rb.CenterOfMassPosition.ToString());
}
}
public void InitGImpactCollision()
{
/// Create Torus Shape
{
m_indexVertexArrays = new TriangleIndexVertexArray(DemoMeshes.TORUS_NUM_TRIANGLES, DemoMeshes.gTorusIndices, 3, DemoMeshes.TORUS_NUM_VERTICES, DemoMeshes.gTorusVertices, 3);
#if BULLET_GIMPACT_CONVEX_DECOMPOSITION
btGImpactConvexDecompositionShape *trimesh = new
btGImpactConvexDecompositionShape(
m_indexVertexArrays, IndexedVector3(1.f, 1.f, 1.f), btScalar(0.01));
trimesh->setMargin(0.07);
trimesh->updateBound();
#else
//GImpactMeshShape trimesh = new GImpactMeshShape(m_indexVertexArrays);
//IndexedVector3 scaling = IndexedVector3.One;
//trimesh.SetLocalScaling(ref scaling);
//trimesh.SetMargin(0.07f); ///?????
//trimesh.UpdateBound();
#endif
//m_trimeshShape = trimesh;
}
/// Create Bunny Shape
{
m_indexVertexArrays2 = new TriangleIndexVertexArray(DemoMeshes.BUNNY_NUM_TRIANGLES, DemoMeshes.gBunnyIndices, 3, DemoMeshes.BUNNY_NUM_VERTICES, DemoMeshes.gBunnyVertices, 3);
#if BULLET_GIMPACT_CONVEX_DECOMPOSITION
btGImpactConvexDecompositionShape *trimesh2 = new
btGImpactConvexDecompositionShape(
m_indexVertexArrays2, IndexedVector3(4.f, 4.f, 4.f), btScalar(0.01));
trimesh2->setMargin(0.07);
trimesh2->updateBound();
#else
GImpactMeshShape trimesh2 = new GImpactMeshShape(m_indexVertexArrays2);
IndexedVector3 scaling = new IndexedVector3(4.0f, 4.0f, 4.0f);
trimesh2.SetLocalScaling(ref scaling);
//trimesh2.SetMargin(0.07f); ///?????
trimesh2.UpdateBound();
#endif
m_trimeshShape2 = trimesh2;
}
///register GIMPACT algorithm
CollisionDispatcher dispatcher = m_dynamicsWorld.GetDispatcher() as CollisionDispatcher;
GImpactCollisionAlgorithm.RegisterAlgorithm(dispatcher);
}
Mesh CreateGImpactMeshShape(GImpactMeshShape shape)
{
BulletSharp.DataStream verts, indices;
int numVerts, numFaces;
PhyScalarType vertsType, indicesType;
int vertexStride, indexStride;
shape.MeshInterface.GetLockedReadOnlyVertexIndexData(out verts, out numVerts, out vertsType, out vertexStride,
out indices, out indexStride, out numFaces, out indicesType);
bool index32 = numVerts > 65536;
Mesh mesh = new Mesh(device, numFaces, numVerts,
MeshFlags.SystemMemory | (index32 ? MeshFlags.Use32Bit : 0), VertexFormat.Position | VertexFormat.Normal);
SlimDX.DataStream vertexBuffer = mesh.LockVertexBuffer(LockFlags.Discard);
while (vertexBuffer.Position < vertexBuffer.Length)
{
vertexBuffer.Write(verts.Read <Vector3>());
vertexBuffer.Position += 12;
}
mesh.UnlockVertexBuffer();
SlimDX.DataStream indexBuffer = mesh.LockIndexBuffer(LockFlags.Discard);
if (index32)
{
while (indexBuffer.Position < indexBuffer.Length)
{
indexBuffer.Write(indices.Read <int>());
}
}
else
{
while (indexBuffer.Position < indexBuffer.Length)
{
indexBuffer.Write((short)indices.Read <int>());
}
}
mesh.UnlockIndexBuffer();
mesh.ComputeNormals();
shapes.Add(shape, mesh);
return(mesh);
}
public static Vector3[] CreateGImpactMesh(GImpactMeshShape shape)
{
DataStream vertexBuffer, indexBuffer;
int numVerts, numFaces;
PhyScalarType vertsType, indicesType;
int vertexStride, indexStride;
shape.MeshInterface.GetLockedReadOnlyVertexIndexData(out vertexBuffer, out numVerts, out vertsType, out vertexStride,
out indexBuffer, out indexStride, out numFaces, out indicesType);
Vector3[] vertices = new Vector3[numFaces * 3 * 2];
// Need to un-index the vertex buffer to make the normals right.
int v = 0;
while (indexBuffer.Position < indexBuffer.Length)
{
uint i = indexBuffer.Read <uint>();
vertexBuffer.Position = vertexStride * i;
Vector3 v0 = vertexBuffer.Read <Vector3>();
i = indexBuffer.Read <uint>();
vertexBuffer.Position = vertexStride * i;
Vector3 v1 = vertexBuffer.Read <Vector3>();
i = indexBuffer.Read <uint>();
vertexBuffer.Position = vertexStride * i;
Vector3 v2 = vertexBuffer.Read <Vector3>();
Vector3 v01 = v0 - v1;
Vector3 v02 = v0 - v2;
Vector3 normal = Vector3.Cross(v01, v02);
normal.Normalize();
vertices[v++] = v0;
vertices[v++] = normal;
vertices[v++] = v1;
vertices[v++] = normal;
vertices[v++] = v2;
vertices[v++] = normal;
}
return(vertices);
}
public override CollisionShape GetCollisionShape()
{
if (collisionShapePtr == null)
{
Vector3[] verts = hullMesh.vertices;
int[] tris = hullMesh.triangles;
//todo test for convex. Make convex if not.
TriangleMesh tm = new TriangleMesh();
for (int i = 0; i < tris.Length; i += 3)
{
tm.AddTriangle(verts[tris[i]].ToBullet(),
verts[tris[i + 1]].ToBullet(),
verts[tris[i + 2]].ToBullet(),
true);
}
collisionShapePtr = new GImpactMeshShape(tm);
((GImpactMeshShape)collisionShapePtr).LocalScaling = m_localScaling.ToBullet();
}
return(collisionShapePtr);
}
public IGImpactMeshShapeImp AddGImpactMeshShape(int[] meshTriangles, float3[] meshVertices)
{
Vector3[] btMeshVertices = new Vector3[meshVertices.Length];
for (int i = 0; i < meshVertices.Length; i++)
{
btMeshVertices[i].X = meshVertices[i].x;
btMeshVertices[i].Y = meshVertices[i].y;
btMeshVertices[i].Z = meshVertices[i].z;
}
var btTriangleIndexVertexArray = new TriangleIndexVertexArray(meshTriangles, btMeshVertices);
var btGimpactMeshShape = new GImpactMeshShape(btTriangleIndexVertexArray);
btGimpactMeshShape.UpdateBound();
BtCollisionShapes.Add(btGimpactMeshShape);
var retval = new GImpactMeshShapeImp();
retval.BtGImpactMeshShape = btGimpactMeshShape;
btGimpactMeshShape.UserObject = retval;
return(retval);
}
public GImpactTestDemoSimulation()
{
CollisionConfiguration = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConfiguration);
//Broadphase = new SimpleBroadphase();
Broadphase = new AxisSweep3_32Bit(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000), 1024);
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConfiguration);
GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
_torusShapeData = new TriangleIndexVertexArray(Torus.Indices, Torus.Vertices);
_torusShape = CreateGImpactShape(_torusShapeData);
_bunnyShapeData = new TriangleIndexVertexArray(Bunny.Indices, Bunny.Vertices);
_bunnyShape = CreateGImpactShape(_bunnyShapeData);
CreateStaticScene();
CreateTorusChain();
CreateBoxes();
}
private CollisionShape GetCollisionShape(cl_entity_t *pEntity)
{
// directly read from cache.
CollisionShape shape = _shapesCache[pEntity->curstate.modelindex];
// cache missing
if (shape == null)
{
if (pEntity->model->type == modtype.mod_brush)
{
int brushIndex = 0;
unsafe
{
// The name looks like "*XX", we ignore '*' and parse to integer.
var p = pEntity->model->name + 1;
while (*p != 0)
{
brushIndex = brushIndex * 10 + (*p - 48);
p++;
}
}
shape = new GImpactMeshShape(_bspModels[brushIndex]);
_shapesCache[pEntity->curstate.modelindex] = shape;
}
else if (pEntity->model->type == modtype.mod_studio)
{
Vector3 aabbSize = (pEntity->curstate.maxs - pEntity->curstate.mins) * GBConstant.G2BScale;
shape = new BoxShape(aabbSize / 2f);
}
else
{
return(null);
}
}
return(shape);
}
/*private void MyTickCallBack(ManifoldPoint cp, CollisionObjectWrapper colobj0wrap, int partid0, int index0, CollisionObjectWrapper colobj1wrap, int partid1, int index1)
{
Debug.WriteLine("MyTickCallBack");
int numManifolds = BtWorld.Dispatcher.NumManifolds;
RigidBodyImp myRb;
//Debug.WriteLine("numManifolds: " + numManifolds);
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
int numContacts = contactManifold.NumContacts;
if (numContacts > 0)
{
CollisionObject obA = (CollisionObject) contactManifold.Body0;
CollisionObject obB = (CollisionObject) contactManifold.Body1;
// Debug.WriteLine(numContacts);
var pnA = obA.UserObject;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
}
}
}
}*/
public IRigidBodyImp AddRigidBody(float mass, float3 worldTransform, float3 orientation, ICollisionShapeImp colShape/*, float3 intertia*/)
{
// Use bullet to do what needs to be done:
var btMatrix = Matrix.RotationX(orientation.x)
* Matrix.RotationY(orientation.y)
* Matrix.RotationZ(orientation.z)
* Matrix.Translation(worldTransform.x, worldTransform.y, worldTransform.z);
var btMotionState = new DefaultMotionState(btMatrix);
var shapeType = colShape.GetType().ToString();
CollisionShape btColShape;
var isStatic = false;
switch (shapeType)
{
//Primitives
case "Fusee.Engine.BoxShapeImp":
var box = (BoxShapeImp) colShape;
var btBoxHalfExtents = Translater.Float3ToBtVector3(box.HalfExtents);
btColShape = new BoxShape(btBoxHalfExtents);
break;
case "Fusee.Engine.CapsuleShapeImp":
var capsule = (CapsuleShapeImp) colShape;
btColShape = new CapsuleShape(capsule.Radius, capsule.HalfHeight);
break;
case "Fusee.Engine.ConeShapeImp":
var cone = (ConeShapeImp) colShape;
btColShape = new ConeShape(cone.Radius, cone.Height);
break;
case "Fusee.Engine.CylinderShapeImp":
var cylinider = (CylinderShapeImp) colShape;
var btCylinderHalfExtents = Translater.Float3ToBtVector3(cylinider.HalfExtents);
btColShape = new CylinderShape(btCylinderHalfExtents);
break;
case "Fusee.Engine.MultiSphereShapeImp":
var multiSphere = (MultiSphereShapeImp) colShape;
var btPositions = new Vector3[multiSphere.SphereCount];
var btRadi = new float[multiSphere.SphereCount];
for (int i = 0; i < multiSphere.SphereCount; i++)
{
var pos = Translater.Float3ToBtVector3(multiSphere.GetSpherePosition(i));
btPositions[i] = pos;
btRadi[i] = multiSphere.GetSphereRadius(i);
}
btColShape = new MultiSphereShape(btPositions, btRadi);
break;
case "Fusee.Engine.SphereShapeImp":
var sphere = (SphereShapeImp) colShape;
var btRadius = sphere.Radius;
btColShape = new SphereShape(btRadius);
break;
//Misc
case "Fusee.Engine.CompoundShapeImp":
var compShape = (CompoundShapeImp) colShape;
btColShape = new CompoundShape(true);
btColShape = compShape.BtCompoundShape;
break;
case "Fusee.Engine.EmptyShapeImp":
btColShape = new EmptyShape();
break;
//Meshes
case "Fusee.Engine.ConvexHullShapeImp":
var convHull = (ConvexHullShapeImp) colShape;
var btPoints= new Vector3[convHull.GetNumPoints()];
for (int i = 0; i < convHull.GetNumPoints(); i++)
{
var point = convHull.GetScaledPoint(i);
btPoints[i] = Translater.Float3ToBtVector3(point);
}
btColShape = new ConvexHullShape(btPoints);
//btColShape.LocalScaling = new Vector3(3,3,3);
break;
case "Fusee.Engine.StaticPlaneShapeImp":
var staticPlane = (StaticPlaneShapeImp) colShape;
Debug.WriteLine("staticplane: " + staticPlane.Margin);
var btNormal = Translater.Float3ToBtVector3(staticPlane.PlaneNormal);
btColShape = new StaticPlaneShape(btNormal, staticPlane.PlaneConstant);
isStatic = true;
//btColShape.Margin = 0.04f;
//Debug.WriteLine("btColshape" + btColShape.Margin);
break;
case "Fusee.Engine.GImpactMeshShapeImp":
var gImpMesh = (GImpactMeshShapeImp)colShape;
gImpMesh.BtGImpactMeshShape.UpdateBound();
var btGimp = new GImpactMeshShape(gImpMesh.BtGImpactMeshShape.MeshInterface);
btGimp.UpdateBound();
btColShape = btGimp;
break;
//Default
default:
Debug.WriteLine("defaultImp");
btColShape = new EmptyShape();
break;
}
var btLocalInertia = btColShape.CalculateLocalInertia(mass);
// btLocalInertia *= (10.0f*10);
RigidBodyConstructionInfo btRbcInfo = new RigidBodyConstructionInfo(mass, btMotionState, btColShape,
btLocalInertia);
var btRigidBody = new RigidBody(btRbcInfo);
btRigidBody.Restitution = 0.2f;
btRigidBody.Friction = 0.2f;
btRigidBody.CollisionFlags = CollisionFlags.CustomMaterialCallback;
BtWorld.AddRigidBody(btRigidBody);
btRbcInfo.Dispose();
var retval = new RigidBodyImp();
retval._rbi = btRigidBody;
btRigidBody.UserObject = retval;
return retval;
}
public IGImpactMeshShapeImp AddGImpactMeshShape(int[] meshTriangles, float3[] meshVertices)
{
Vector3[] btMeshVertices = new Vector3[meshVertices.Length];
for (int i = 0; i < meshVertices.Length; i++)
{
btMeshVertices[i].X = meshVertices[i].x;
btMeshVertices[i].Y = meshVertices[i].y;
btMeshVertices[i].Z = meshVertices[i].z;
}
var btTriangleIndexVertexArray = new TriangleIndexVertexArray(meshTriangles, btMeshVertices);
var btGimpactMeshShape = new GImpactMeshShape(btTriangleIndexVertexArray);
btGimpactMeshShape.UpdateBound();
BtCollisionShapes.Add(btGimpactMeshShape);
var retval = new GImpactMeshShapeImp();
retval.BtGImpactMeshShape = btGimpactMeshShape;
btGimpactMeshShape.UserObject = retval;
return retval;
}
protected void SetupSim()
{
// randomly choose scaling before resetting rigid body
float randomMass = 0;
BulletSharp.Math.Vector3 randomInertia;
if (randomScale)
{
if (varyScale)
{
m_pclScale.x = Random.Range(this.scaleMin, this.scaleMax);
m_pclScale.y = Random.Range(this.scaleMin, this.scaleMax);
m_pclScale.z = Random.Range(this.scaleMin, this.scaleMax);
}
else
{
float uniformScale = Random.Range(this.scaleMin, this.scaleMax);
m_pclScale.x = uniformScale;
m_pclScale.y = uniformScale;
m_pclScale.z = uniformScale;
}
//// z can't be more than thrice or less than half of x scale
//float zmin = Mathf.Max(this.scaleMin, 0.5f * m_pclScale.x);
//float zmax = Mathf.Min(this.scaleMax, 3.0f * m_pclScale.x);
//randomScale = Random.Range(zmin, zmax);
//m_pclScale.z = randomScale;
//// y can't be greater than 2 times the smallest of x and z
//float ymax = 2.0f * Mathf.Min(m_pclScale.x, m_pclScale.z);
//randomScale = Random.Range(this.scaleMin, Mathf.Min(ymax, this.scaleMax));
//m_pclScale.y = randomScale;
if (DEBUG)
{
Debug.Log("Scaling by " + m_pclScale.ToString());
}
// randomMass = m_masses[m_curObjIdx] * m_pclScale.x * m_pclScale.y * m_pclScale.z;
float randomDensity;
if (useConstantDensity)
{
// density is constant so mass must scale with volume
randomDensity = densityMin;
randomMass = randomDensity * m_masses[m_curObjIdx] * m_pclScale.x * m_pclScale.y * m_pclScale.z;
}
else
{
randomDensity = Random.Range(densityMin, densityMax);
randomMass = randomDensity * m_masses[m_curObjIdx];
}
// inertia must scale with volume no matter if the density is constant or not
BulletSharp.Math.Vector3 objInertiaInfo = m_inertias[m_curObjIdx];
float scalexyz = m_pclScale.x * m_pclScale.y * m_pclScale.z;
float scalex2 = m_pclScale.x * m_pclScale.x;
float scaley2 = m_pclScale.y * m_pclScale.y;
float scalez2 = m_pclScale.z * m_pclScale.z;
float inertiax = randomDensity * scalexyz * (scaley2 * objInertiaInfo[1] + scalez2 * objInertiaInfo[2]);
float inertiay = randomDensity * scalexyz * (scalex2 * objInertiaInfo[0] + scalez2 * objInertiaInfo[2]);
float inertiaz = randomDensity * scalexyz * (scalex2 * objInertiaInfo[0] + scaley2 * objInertiaInfo[1]);
randomInertia = new BulletSharp.Math.Vector3(inertiax, inertiay, inertiaz);
// need to completely destory rigid body because need new mass/moment of inertia
DestroySimObj();
DataGenUtils.BulletOBJMesh scaledMesh = m_btmesh.Scale(m_pclScale.x, m_pclScale.y, m_pclScale.z);
var triVtxarray = new TriangleIndexVertexArray(scaledMesh.indices, scaledMesh.vertices);
m_cs = new GImpactMeshShape(triVtxarray);
m_cs.LocalScaling = new BulletSharp.Math.Vector3(1);
m_cs.Margin = bodyMargin;
m_cs.UpdateBound();
AddCollisionShape(m_cs);
// move it up so resting on the ground plane
float miny = float.MaxValue;
float maxz = float.MinValue;
float cury;
float curz;
for (int i = 0; i < scaledMesh.vertices.Length / 3; i++)
{
cury = scaledMesh.vertices[i * 3 + 1];
if (cury < miny)
{
miny = cury;
}
curz = scaledMesh.vertices[i * 3 + 2];
if (curz > maxz)
{
maxz = curz;
}
}
miny = -miny;
m_rbInitTransVec = new BulletSharp.Math.Vector3(0, miny + bodyMargin + m_groundMargin, 0);
m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); //* Matrix.RotationY(Random.Range(0.0f, 360.0f));
//float gtInertiaX = (1.0f / 12.0f) * randomMass * (3.0f * maxz * maxz + (2.0f * miny) * (2.0f * miny));
//float gtInertiaZ = gtInertiaX;
//float gtInertiaY = 0.5f * randomMass * maxz * maxz;
//BulletSharp.Math.Vector3 gtInertia = new BulletSharp.Math.Vector3(gtInertiaX, gtInertiaY, gtInertiaZ);
//Debug.Log("GT INERTIA: " + gtInertia.ToString());
//randomInertia = gtInertia;
m_rb = CreateRigidBody(randomMass, randomInertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction, viz: RENDER_MODE);
//m_rb = CreateRigidBody(randomMass, m_rbInitTrans, m_cs, bodyMat, bodyFriction);
m_rb.AngularFactor = angularFactor;
m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
m_mass = randomMass;
m_inertia = randomInertia;
m_density = randomDensity;
}
else
{
// using the same mesh just need to choose a new density
// steps for determinism
// https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=3143
float randomDensity;
if (useConstantDensity)
{
randomDensity = densityMin;
}
else
{
randomDensity = Random.Range(densityMin, densityMax);
}
randomMass = randomDensity * m_masses[m_curObjIdx];
BulletSharp.Math.Vector3 objInertiaInfo = m_inertias[m_curObjIdx];
float inertiax = randomDensity * (objInertiaInfo[1] + objInertiaInfo[2]);
float inertiay = randomDensity * (objInertiaInfo[0] + objInertiaInfo[2]);
float inertiaz = randomDensity * (objInertiaInfo[0] + objInertiaInfo[1]);
randomInertia = new BulletSharp.Math.Vector3(inertiax, inertiay, inertiaz);
m_rb.SetMassProps(randomMass, randomInertia);
m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); // * Matrix.RotationY(Random.Range(0.0f, 360.0f));
m_rb = ResetRigidBody(m_rb, randomMass, randomInertia, m_rbInitTrans, m_cs, bodyFriction);
m_rb.AngularFactor = angularFactor;
m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
// HingeConstraint hingeConstraint = new HingeConstraint(m_rb, new BulletSharp.Math.Vector3(0.0f), new BulletSharp.Math.Vector3(0.0f, 1.0f, 0.0f), false);
// m_world.AddConstraint(hingeConstraint);
// DestroySimObj(); // have to do this to set mass properties but can reuse previously calculated everything else
// if (m_cs == null) Debug.Log("NOT NULL");
// m_rb = CreateRigidBody(randomMass, randomInertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction);
// m_rb.AngularFactor = new BulletSharp.Math.Vector3(angularFactor);
// m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
m_mass = randomMass;
m_inertia = randomInertia;
m_density = randomDensity;
}
m_stepCount = 0;
m_broadphase.ResetPool(m_colDispatcher);
m_solver.Reset();
float curMass = 1.0f / m_rb.InvMass;
if (DEBUG)
{
Debug.Log("Mass: " + curMass.ToString());
}
if (DEBUG)
{
Debug.Log("LOCAL MOMENT: " + m_rb.LocalInertia.ToString());
}
if (DEBUG)
{
Debug.Log("COM " + m_rb.CenterOfMassPosition.ToString());
}
if (DEBUG)
{
Debug.Log("Density " + m_density.ToString());
}
// determine impulse position
ClosestRayResultCallback cb;
BulletSharp.Math.Vector3 vertexNormal = new BulletSharp.Math.Vector3();
int missCount = 0;
do
{
// choose random vertex to apply force to
// pick a random point around in the plane around y position
float offsetx = UnityEngine.Random.Range(-100.0f, 100.0f);
float offsetz = UnityEngine.Random.Range(-100.0f, 100.0f);
Vector2 offsetvec = new Vector2(offsetx, offsetz);
// offsetvec.Normalize();
//float relForceHeight = 0.75f;
UnityEngine.Vector3 offsetPt = new UnityEngine.Vector3(offsetvec[0],
m_rb.CenterOfMassPosition.Y,
offsetvec[1]);
BulletSharp.Math.Vector3 btOffsetPt = BSExtensionMethods2.ToBullet(offsetPt);
BulletSharp.Math.Vector3 btInnerPt = m_rb.CenterOfMassPosition;
cb = new ClosestRayResultCallback(ref btOffsetPt, ref btInnerPt);
// Debug.DrawLine(BSExtensionMethods2.ToUnity(btInnerPt), offsetPt, Color.red, 2.0f);
m_world.RayTest(btOffsetPt, btInnerPt, cb);
if (cb.HasHit)
{
m_forcePoint = cb.HitPointWorld;
vertexNormal = cb.HitNormalWorld;
}
else
{
missCount++;
//Debug.Log("ERROR - couldn't find point to apply force to. Retrying...");
//return;
}
} while (!cb.HasHit);
if (DEBUG)
{
Debug.Log("Missed impulse " + missCount.ToString() + " times.");
}
if (DEBUG)
{
Debug.LogFormat("ForcePoint: " + m_forcePoint.ToString());
}
// get force vector
// loop until force is applied to outside of object
UnityEngine.Vector3 uForceVec = new UnityEngine.Vector3();
// initialize force vector to coincide with center of mass
BulletSharp.Math.Vector3 btForceVec = m_rb.CenterOfMassPosition - m_forcePoint;
// then randomly vary it within the x/z plane to be within the specified distance
BulletSharp.Math.Vector3 btVariationVec = new BulletSharp.Math.Vector3(-btForceVec[2], 0.0f, btForceVec[0]);
btVariationVec.Normalize();
float varyForce;
BulletSharp.Math.Vector3 proposedForceVec;
do
{
varyForce = UnityEngine.Random.Range(-forceDistMax, forceDistMax);
proposedForceVec = btVariationVec * varyForce + btForceVec;
} while (proposedForceVec.Dot(vertexNormal) >= 0); // must also be on the outside of the object
btForceVec = proposedForceVec;
btForceVec.Normalize();
uForceVec = BSExtensionMethods2.ToUnity(btForceVec);
if (DEBUG)
{
Debug.Log("FORCE DIST: " + varyForce.ToString());
}
//UnityEngine.Vector3 uVtxNormal = BSExtensionMethods2.ToUnity(vertexNormal);
//uVtxNormal.Normalize();
//do
//{
// float forcex = UnityEngine.Random.Range(-1.0f, 1.0f);
// float forcez = UnityEngine.Random.Range(-1.0f, 1.0f);
// uForceVec.Set(forcex, 0.0f, forcez);
// uForceVec.Normalize();
//} while (UnityEngine.Vector3.Dot(uForceVec, uVtxNormal) >= 0);
// random constrained magnitude
float mag = UnityEngine.Random.Range(impulseMin, impulseMax);
//Debug.Log("Vol: " + objectVolume.ToString());
// if (varyScale) {
// mag *= randomMass; // scale impulse t unity
//according to object scale
// } else {
// mag *= curMass;
// }
mag *= m_mass; // scale impulse according to object mass
uForceVec *= mag;
// set directly for debugging
//uForceVec.Set(2.5f, 0.0f, 0.0f);
//m_forcePoint = new BulletSharp.Math.Vector3(0.0f, m_rb.CenterOfMassPosition.Y, -0.15f);
m_forceVec = BSExtensionMethods2.ToBullet(uForceVec);
if (DEBUG)
{
Debug.LogFormat("ForceVec: " + m_forceVec.ToString());
}
if (DEBUG)
{
UnityEngine.Vector3 debugVec = -uForceVec;
debugVec.Scale(new UnityEngine.Vector3(0.5f, 0.5f, 0.5f));
Debug.DrawRay(BSExtensionMethods2.ToUnity(m_forcePoint), debugVec, Color.green, 1.0f);
Debug.DrawLine(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), BSExtensionMethods2.ToUnity(m_forcePoint), Color.cyan, 1.0f);
Debug.DrawLine(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition) + BSExtensionMethods2.ToUnity(btVariationVec) * varyForce, Color.blue, 1.0f);
}
// apply the random impulse
BulletSharp.Math.Vector3 radius = m_forcePoint - m_rb.CenterOfMassPosition;
m_rb.ApplyImpulse(m_forceVec, radius);
// m_rb.ApplyTorqueImpulse(new BulletSharp.Math.Vector3(0.0f, 1.0f, 0.0f));
// m_rb.ApplyCentralImpulse(new BulletSharp.Math.Vector3(4.0f, 0.0f, 2.0f));
// BulletSharp.Math.Vector3 newAngVel = m_rb.AngularVelocity;
// newAngVel.X = 0.0f;
// newAngVel.Z = 0.0f;
// m_rb.AngularVelocity = newAngVel;
// calculate ground truth for debugging
//BulletSharp.Math.Vector3 gtAngVel = radius.Cross(m_forceVec) / m_inertia;
//BulletSharp.Math.Vector3 gtLinVel = m_forceVec / m_mass;
//Debug.Log("GT LIN VEL: " + gtLinVel.ToString());
//Debug.Log("GT ANG VEL: " + gtAngVel.ToString());
}
/*private void MyTickCallBack(ManifoldPoint cp, CollisionObjectWrapper colobj0wrap, int partid0, int index0, CollisionObjectWrapper colobj1wrap, int partid1, int index1)
* {
* Debug.WriteLine("MyTickCallBack");
* int numManifolds = BtWorld.Dispatcher.NumManifolds;
* RigidBodyImp myRb;
* //Debug.WriteLine("numManifolds: " + numManifolds);
* for (int i = 0; i < numManifolds; i++)
* {
* PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
* int numContacts = contactManifold.NumContacts;
* if (numContacts > 0)
* {
* CollisionObject obA = (CollisionObject) contactManifold.Body0;
* CollisionObject obB = (CollisionObject) contactManifold.Body1;
*
* // Debug.WriteLine(numContacts);
* var pnA = obA.UserObject;
*
* for (int j = 0; j < numContacts; j++)
* {
* ManifoldPoint pt = contactManifold.GetContactPoint(j);
*
* }
* }
* }
* }*/
public IRigidBodyImp AddRigidBody(float mass, float3 worldTransform, float3 orientation, ICollisionShapeImp colShape /*, float3 intertia*/)
{
// Use bullet to do what needs to be done:
var btMatrix = Matrix.RotationX(orientation.x)
* Matrix.RotationY(orientation.y)
* Matrix.RotationZ(orientation.z)
* Matrix.Translation(worldTransform.x, worldTransform.y, worldTransform.z);
var btMotionState = new DefaultMotionState(btMatrix);
var shapeType = colShape.GetType().ToString();
CollisionShape btColShape;
var isStatic = false;
switch (shapeType)
{
//Primitives
case "Fusee.Engine.BoxShapeImp":
var box = (BoxShapeImp)colShape;
var btBoxHalfExtents = Translater.Float3ToBtVector3(box.HalfExtents);
btColShape = new BoxShape(btBoxHalfExtents);
break;
case "Fusee.Engine.CapsuleShapeImp":
var capsule = (CapsuleShapeImp)colShape;
btColShape = new CapsuleShape(capsule.Radius, capsule.HalfHeight);
break;
case "Fusee.Engine.ConeShapeImp":
var cone = (ConeShapeImp)colShape;
btColShape = new ConeShape(cone.Radius, cone.Height);
break;
case "Fusee.Engine.CylinderShapeImp":
var cylinider = (CylinderShapeImp)colShape;
var btCylinderHalfExtents = Translater.Float3ToBtVector3(cylinider.HalfExtents);
btColShape = new CylinderShape(btCylinderHalfExtents);
break;
case "Fusee.Engine.MultiSphereShapeImp":
var multiSphere = (MultiSphereShapeImp)colShape;
var btPositions = new Vector3[multiSphere.SphereCount];
var btRadi = new float[multiSphere.SphereCount];
for (int i = 0; i < multiSphere.SphereCount; i++)
{
var pos = Translater.Float3ToBtVector3(multiSphere.GetSpherePosition(i));
btPositions[i] = pos;
btRadi[i] = multiSphere.GetSphereRadius(i);
}
btColShape = new MultiSphereShape(btPositions, btRadi);
break;
case "Fusee.Engine.SphereShapeImp":
var sphere = (SphereShapeImp)colShape;
var btRadius = sphere.Radius;
btColShape = new SphereShape(btRadius);
break;
//Misc
case "Fusee.Engine.CompoundShapeImp":
var compShape = (CompoundShapeImp)colShape;
btColShape = new CompoundShape(true);
btColShape = compShape.BtCompoundShape;
break;
case "Fusee.Engine.EmptyShapeImp":
btColShape = new EmptyShape();
break;
//Meshes
case "Fusee.Engine.ConvexHullShapeImp":
var convHull = (ConvexHullShapeImp)colShape;
var btPoints = new Vector3[convHull.GetNumPoints()];
for (int i = 0; i < convHull.GetNumPoints(); i++)
{
var point = convHull.GetScaledPoint(i);
btPoints[i] = Translater.Float3ToBtVector3(point);
}
btColShape = new ConvexHullShape(btPoints);
//btColShape.LocalScaling = new Vector3(3,3,3);
break;
case "Fusee.Engine.StaticPlaneShapeImp":
var staticPlane = (StaticPlaneShapeImp)colShape;
Debug.WriteLine("staticplane: " + staticPlane.Margin);
var btNormal = Translater.Float3ToBtVector3(staticPlane.PlaneNormal);
btColShape = new StaticPlaneShape(btNormal, staticPlane.PlaneConstant);
isStatic = true;
//btColShape.Margin = 0.04f;
//Debug.WriteLine("btColshape" + btColShape.Margin);
break;
case "Fusee.Engine.GImpactMeshShapeImp":
var gImpMesh = (GImpactMeshShapeImp)colShape;
gImpMesh.BtGImpactMeshShape.UpdateBound();
var btGimp = new GImpactMeshShape(gImpMesh.BtGImpactMeshShape.MeshInterface);
btGimp.UpdateBound();
btColShape = btGimp;
break;
//Default
default:
Debug.WriteLine("defaultImp");
btColShape = new EmptyShape();
break;
}
var btLocalInertia = btColShape.CalculateLocalInertia(mass);
// btLocalInertia *= (10.0f*10);
RigidBodyConstructionInfo btRbcInfo = new RigidBodyConstructionInfo(mass, btMotionState, btColShape,
btLocalInertia);
var btRigidBody = new RigidBody(btRbcInfo);
btRigidBody.Restitution = 0.2f;
btRigidBody.Friction = 0.2f;
btRigidBody.CollisionFlags = CollisionFlags.CustomMaterialCallback;
BtWorld.AddRigidBody(btRigidBody);
btRbcInfo.Dispose();
var retval = new RigidBodyImp();
retval._rbi = btRigidBody;
btRigidBody.UserObject = retval;
return(retval);
}
