/// <summary>
/// Se crea uncuerpo rigido a partir de un TgcMesh, pero no tiene masa por lo que va a ser estatico.
/// </summary>
/// <param name="mesh">TgcMesh</param>
/// <returns>Cuerpo rigido de un Mesh</returns>
public RigidBody CreateRigidBodyFromTgcMesh(TgcMesh mesh)
{
var vertexCoords = mesh.getVertexPositions();
TriangleMesh triangleMesh = new TriangleMesh();
for (int i = 0; i < vertexCoords.Length; i = i + 3)
{
triangleMesh.AddTriangle(vertexCoords[i].ToBulletVector3(), vertexCoords[i + 1].ToBulletVector3(), vertexCoords[i + 2].ToBulletVector3());
}
var transformationMatrix = TGCMatrix.RotationYawPitchRoll(0, 0, 0).ToBsMatrix;
DefaultMotionState motionState = new DefaultMotionState(transformationMatrix);
var bulletShape = new BvhTriangleMeshShape(triangleMesh, false);
var boxLocalInertia = bulletShape.CalculateLocalInertia(0);
var bodyInfo = new RigidBodyConstructionInfo(0, motionState, bulletShape, boxLocalInertia);
var rigidBody = new RigidBody(bodyInfo);
rigidBody.Friction = 0.4f;
rigidBody.RollingFriction = 1;
rigidBody.Restitution = 1f;
return(rigidBody);
}
protected override CollisionShape CreateShape()
{
TriangleIndexVertexArray tiv = new TriangleIndexVertexArray(this.indices, this.vertices);
BvhTriangleMeshShape bvh = new BvhTriangleMeshShape(tiv, true, true);
return(bvh);
}
public ConvexDecompositionDemoSimulation(bool enableSat)
{
_enableSat = enableSat;
CollisionConfiguration = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConfiguration);
Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConfiguration);
CreateGround();
ManifoldPoint.ContactAdded += MyContactCallback;
//CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
string path = Path.Combine("data", "file.obj");
var wavefrontModel = WavefrontObj.Load(path);
if (wavefrontModel.Indices.Count == 0)
{
return;
}
var localScaling = new Vector3(6, 6, 6);
_triangleMesh = CreateTriangleMesh(wavefrontModel.Indices, wavefrontModel.Vertices, localScaling);
// Convex hull approximation
ConvexHullShape convexShape = CreateHullApproximation(_triangleMesh);
float mass = 1.0f;
PhysicsHelper.CreateBody(mass, Matrix.Translation(0, 2, 14), convexShape, World);
// Non-moving body
var objectOffset = new Vector3(10, 0, 0);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(_triangleMesh, useQuantization);
PhysicsHelper.CreateStaticBody(Matrix.Translation(objectOffset), concaveShape, World);
Hacd hacd = ComputeHacd(wavefrontModel);
hacd.Save("output.wrl", false);
var compoundShape = CreateCompoundShape(hacd, localScaling);
mass = 10.0f;
objectOffset = new Vector3(-10, 0, -6);
var body2 = PhysicsHelper.CreateBody(mass, Matrix.Translation(objectOffset), compoundShape, World);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
objectOffset.Z += 6;
body2 = PhysicsHelper.CreateBody(mass, Matrix.Translation(objectOffset), compoundShape, World);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
objectOffset.Z += 6;
body2 = PhysicsHelper.CreateBody(mass, Matrix.Translation(objectOffset), compoundShape, World);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
}
private void CreateMeshBody(TriangleMesh mesh, Matrix transform)
{
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(mesh, useQuantization);
PhysicsHelper.CreateStaticBody(transform, concaveShape, World);
}
public void AddShape(Mesh mesh, Transform tran)
{
// Cube, Plane은 잘 된다. 구는 잘 안 되는데
// C++로 불릿 내부를 본 뒤에 다시 보도록 한다.
// TriangleMesh는 정적 노드를 기준으로 한다.
// 변환해서 넣어야 됨. WorldTransform 반영 안 함
var triMesh = new TriangleMesh(true, true);
for (int i = 0; i < mesh.Indices.Count; i += 3)
{
var v0 = ToBulletVector3(OpenTK.Vector3.TransformPosition(mesh.Vertices[mesh.Indices[i + 0]].Position, tran.Matrix));
var v1 = ToBulletVector3(OpenTK.Vector3.TransformPosition(mesh.Vertices[mesh.Indices[i + 1]].Position, tran.Matrix));
var v2 = ToBulletVector3(OpenTK.Vector3.TransformPosition(mesh.Vertices[mesh.Indices[i + 2]].Position, tran.Matrix));
triMesh.AddTriangle(v0, v1, v2, false);
}
var triShape = new BvhTriangleMeshShape(triMesh, true, true);
// 아래는 생략해도 피킹 잘 됨
// triShape.BuildOptimizedBvh();
var colObj = new CollisionObject();
colObj.CollisionShape = triShape;
colObj.WorldTransform = ToBulletMatrix(OpenTK.Matrix4.Identity);
world.AddCollisionObject(colObj);
world.StepSimulation(0.1f);
}
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);
}
public override void OnDrawGizmosSelected()
{
if (!drawGizmo)
{
return;
}
Gizmos.color = Color.yellow;
CollisionShape shape = GetCollisionShape();
ConvexHullShape convexShape = shape as ConvexHullShape;
Gizmos.matrix = transform.localToWorldMatrix * Matrix4x4.Scale(transform.lossyScale).inverse;
if (convexShape != null)
{
//BulletSharp.Math.Matrix childShapeTransform = this.transform;
//childShapeTransform.Invert();
//BulletSharp.Math.Matrix shapeTransform = childShapeTransform * this.transform.localToWorldMatrix.ToBullet();
int nbEdges = convexShape.NumEdges;
for (int j = 0; j < nbEdges; j++)
{
BulletSharp.Math.Vector3 vertex1;
BulletSharp.Math.Vector3 vertex2;
convexShape.GetEdge(j, out vertex1, out vertex2);
Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1);
Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2);
Gizmos.DrawLine(vertexUnity1, vertexUnity2);
}
}
BvhTriangleMeshShape triangleShape = shape as BvhTriangleMeshShape;
if (triangleShape != null)
{
DisplayTriangleCallback cb = new DisplayTriangleCallback();
triangleShape.MeshInterface.InternalProcessAllTriangles(cb, triangleShape.LocalAabbMin, triangleShape.LocalAabbMax);
}
}
internal static object CreateMeshShapeF(object pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
{
ObjectArray <int> indicesarr = new ObjectArray <int>(indices);
ObjectArray <float> vertices = new ObjectArray <float>(verticesAsFloats);
var world = pWorld as DiscreteDynamicsWorld;
IndexedMesh mesh = new IndexedMesh();
mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
mesh.m_numTriangles = pIndicesCount / 3;
mesh.m_numVertices = pVerticesCount;
mesh.m_triangleIndexBase = indicesarr;
mesh.m_vertexBase = vertices;
mesh.m_vertexStride = 3;
mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
mesh.m_triangleIndexStride = 3;
TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, false, true);
//TriangleMeshShape meshShape = new TriangleMeshShape(tribuilder);
//meshShape.SetMargin(world.WorldSettings.Params.collisionMargin);
float margin = 0.02f;
meshShape.SetMargin(margin);
return(meshShape);
}
/// <summary>
/// Starts the component.
/// </summary>
/// <param name="time">Time snapshot</param>
protected override void OnStart(GameTime time)
{
base.OnStart(time);
while (!this.Model.IsLoaded)
{
Thread.Sleep(0);
}
List <Vector3> positions = new List <Vector3>();
List <int> indexList = new List <int>();
int offset = 0;
foreach (var mesh in this.Model.GetMeshes())
{
offset = positions.Count;
foreach (var vertex in mesh.GetVertices())
{
positions.Add(new Vector3(vertex.Position.X, vertex.Position.Y, vertex.Position.Z));
}
uint[] indexes = mesh.GetIndexes();
for (int i = 0; i < indexes.Length; i++)
{
indexList.Add(offset + (int)indexes[i]);
}
}
var triangleMesh = new TriangleIndexVertexArray(indexList.ToArray(), positions.ToArray());
this.shape = new BvhTriangleMeshShape(triangleMesh, true, true);
this.shape.UserObject = this;
}
/// <summary>
/// Crea una coleccion de triangulos para Bullet a partir de los triangulos generados por un heighmap
/// o una coleccion de triangulos a partir de un Custom Vertex Buffer con vertices del tipo Position Texured.
/// Se utilizo el codigo de un snippet de Bullet http://www.bulletphysics.org/mediawiki-1.5.8/index.php?title=Code_Snippets
/// </summary>
/// <param name="triangleDataVB">Custom Vertex Buffer que puede ser de un Heightmap</param>
/// <returns>Rigid Body del terreno</returns>
public static RigidBody CreateSurfaceFromHeighMap(CustomVertex.PositionTextured[] triangleDataVB)
{
//Triangulos
var triangleMesh = new TriangleMesh();
int i = 0;
TGCVector3 vector0;
TGCVector3 vector1;
TGCVector3 vector2;
while (i < triangleDataVB.Length)
{
var triangle = new Triangle();
vector0 = new TGCVector3(triangleDataVB[i].X, triangleDataVB[i].Y, triangleDataVB[i].Z);
vector1 = new TGCVector3(triangleDataVB[i + 1].X, triangleDataVB[i + 1].Y, triangleDataVB[i + 1].Z);
vector2 = new TGCVector3(triangleDataVB[i + 2].X, triangleDataVB[i + 2].Y, triangleDataVB[i + 2].Z);
i = i + 3;
triangleMesh.AddTriangle(vector0.ToBsVector, vector1.ToBsVector, vector2.ToBsVector, false);
}
CollisionShape meshCollisionShape = new BvhTriangleMeshShape(triangleMesh, true);
var meshMotionState = new DefaultMotionState();
var meshRigidBodyInfo = new RigidBodyConstructionInfo(0, meshMotionState, meshCollisionShape);
RigidBody meshRigidBody = new RigidBody(meshRigidBodyInfo);
return(meshRigidBody);
}
/// <summary>
/// Crea una coleccion de triangulos para Bullet a partir de los triangulos generados por un heighmap
/// </summary>
/// <param name="triangleDataVB"></param>
/// <returns></returns>
public static RigidBody CreateSurfaceFromHeighMap(CustomVertex.PositionTextured[] triangleDataVB)
{
/*
* This come from a bullet page
* http://www.bulletphysics.org/mediawiki-1.5.8/index.php?title=Code_Snippets
* btTriangleMesh *mTriMesh = new btTriangleMesh();
*
* while(!done) {
* // For whatever your source of triangles is
* // give the three points of each triangle:
* btVector3 v0(x0,y0,z0);
* btVector3 v1(x1,y1,z1);
* btVector3 v2(x2,y2,z2);
*
* // Then add the triangle to the mesh:
* mTriMesh->addTriangle(v0,v1,v2);
* }
*
* btCollisionShape *mTriMeshShape = new btBvhTriangleMeshShape(mTriMesh,true);
*
* // Now use mTriMeshShape as your collision shape.
* // Everything else is like a normal rigid body
*/
/*
* Para 1 solo triangulo
* var triangle = new Triangle();
* TGCVector3 vector0 = new TGCVector3(0, 0, 0);
* TGCVector3 vector1 = new TGCVector3(100, 0, 0);
* TGCVector3 vector2 = new TGCVector3(0, 0, 100);
*
* triangleMesh.AddTriangle(vector0.ToBsVector,vector1.ToBsVector,vector2.ToBsVector,false);
*/
//Triangulos
var triangleMesh = new TriangleMesh();
int i = 0;
TGCVector3 vector0;
TGCVector3 vector1;
TGCVector3 vector2;
while (i < triangleDataVB.Length)
{
var triangle = new Triangle();
vector0 = new TGCVector3(triangleDataVB[i].X, triangleDataVB[i].Y, triangleDataVB[i].Z);
vector1 = new TGCVector3(triangleDataVB[i + 1].X, triangleDataVB[i + 1].Y, triangleDataVB[i + 1].Z);
vector2 = new TGCVector3(triangleDataVB[i + 2].X, triangleDataVB[i + 2].Y, triangleDataVB[i + 2].Z);
i = i + 3;
triangleMesh.AddTriangle(vector0.ToBsVector, vector1.ToBsVector, vector2.ToBsVector, false);
}
CollisionShape meshCollisionShape = new BvhTriangleMeshShape(triangleMesh, true);
var meshMotionState = new DefaultMotionState();
var meshRigidBodyInfo = new RigidBodyConstructionInfo(0, meshMotionState, meshCollisionShape);
RigidBody meshRigidBody = new RigidBody(meshRigidBodyInfo);
return(meshRigidBody);
}
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();
}
public BvhTriangleMeshShape GetAccurateCollisionShape()
{
//if (CachedBvhTriangleMeshShape != null) return CachedBvhTriangleMeshShape;
List <Vector3> vectors = GetRawVertexList();
var smesh = new TriangleIndexVertexArray(Enumerable.Range(0, Vertices.Count).ToArray(), vectors.Select((a) => a).ToArray());
CachedBvhTriangleMeshShape = new BvhTriangleMeshShape(smesh, false);
//CachedBvhTriangleMeshShape.LocalScaling = new Vector3(scale);
return(CachedBvhTriangleMeshShape);
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
//CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
string path = Path.Combine("data", "file.obj");
var wavefrontModel = WavefrontObj.Load(path);
if (wavefrontModel.Indices.Count == 0)
{
return;
}
var localScaling = new Vector3(6, 6, 6);
_triangleMesh = CreateTriangleMesh(wavefrontModel.Indices, wavefrontModel.Vertices, localScaling);
// Convex hull approximation
ConvexHullShape convexShape = CreateHullApproximation(_triangleMesh);
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
// Non-moving body
var objectOffset = new Vector3(10, 0, 0);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(_triangleMesh, useQuantization);
LocalCreateRigidBody(0, Matrix.Translation(objectOffset), concaveShape);
Hacd hacd = ComputeHacd(wavefrontModel);
hacd.Save("output.wrl", false);
var compoundShape = CreateCompoundShape(hacd, localScaling);
mass = 10.0f;
objectOffset = new Vector3(-10, 0, -6);
var body2 = LocalCreateRigidBody(mass, Matrix.Translation(objectOffset), compoundShape);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
objectOffset.Z += 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(objectOffset), compoundShape);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
objectOffset.Z += 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(objectOffset), compoundShape);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
}
protected override CollisionShape CreateShape()
{
//ConvexHullShape shape = new ConvexHullShape(this.vertices);
//BvhTriangleMeshShape bvh = new BvhTriangleMeshShape(
//StridingMeshInterface smi = new StridingMeshInterface();
TriangleIndexVertexArray tiv = new TriangleIndexVertexArray(this.indices, this.vertices);
BvhTriangleMeshShape bvh = new BvhTriangleMeshShape(tiv, true, true);
//StridingMeshInterface
return(bvh);
}
public static libTechCollisionShape FromVerticesConcave(IEnumerable <Vector3> Verts)
{
Vector3[] VertsArray = Verts.ToArray();
TriangleMesh TriMesh = new TriangleMesh();
for (int i = 0; i < VertsArray.Length; i += 3)
{
TriMesh.AddTriangle(VertsArray[i + 0], VertsArray[i + 1], VertsArray[i + 2]);
}
BvhTriangleMeshShape TriShape = new BvhTriangleMeshShape(TriMesh, false);
return(new libTechCollisionShape(TriShape));
}
public static CollisionShape GetCollisionShapeFromCollider(Collider collider)
{
MeshCollider meshCollider = collider as MeshCollider;
if (meshCollider != null)
{
try
{
Vector3[] vertices = meshCollider.sharedMesh.vertices;
if (meshCollider.convex)
{
float[] points = new float[vertices.Length * 3];
for (int i = 0; i < vertices.Length; ++i)
{
int idx = i * 3;
points[idx] = vertices[i].x;
points[idx + 1] = vertices[i].y;
points[idx + 2] = vertices[i].z;
}
ConvexHullShape cs = new ConvexHullShape(points);
cs.LocalScaling = meshCollider.transform.lossyScale.ToBullet();
cs.Margin = 0f;
//TODO
// cs.OptimizeConvexHull();
return(cs);
}
else
{
int[] tris = meshCollider.sharedMesh.triangles;
TriangleMesh tm = new TriangleMesh();
for (int i = 0; i < tris.Length; i += 3)
{
tm.AddTriangle(vertices[tris[i]].ToBullet(),
vertices[tris[i + 1]].ToBullet(),
vertices[tris[i + 2]].ToBullet(),
true);
}
BvhTriangleMeshShape ts = new BvhTriangleMeshShape(tm, true);
ts.LocalScaling = meshCollider.transform.lossyScale.ToBullet();
return(ts);
}
}
catch (NullReferenceException e)
{
Debug.LogError("Could not get collision shape from collider. Maybe it's not a mesh collider ?", collider);
}
}
return(null);
}
private RigidBody construirRigidBodyDeTriangleMeshShape(BvhTriangleMeshShape triangleMeshShape)
{
var transformationMatrix = TGCMatrix.RotationYawPitchRoll(0, 0, 0).ToBsMatrix;
DefaultMotionState motionState = new DefaultMotionState(transformationMatrix);
var boxLocalInertia = triangleMeshShape.CalculateLocalInertia(0);
var bodyInfo = new RigidBodyConstructionInfo(0, motionState, triangleMeshShape, boxLocalInertia);
var rigidBody = new RigidBody(bodyInfo);
rigidBody.Friction = 0.4f;
rigidBody.RollingFriction = 1;
rigidBody.Restitution = 1f;
return(rigidBody);
}
private void CreateGround()
{
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
const int triangleIndexStride = 3 * sizeof(int);
const int vertexStride = Vector3.SizeInBytes;
var mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts, triangleIndexStride, vertexStride);
var indicesStream = mesh.GetTriangleStream();
using (var indices = new BinaryWriter(indicesStream))
{
for (int x = 0; x < NumVertsX - 1; x++)
{
for (int y = 0; y < NumVertsY - 1; y++)
{
int row1Index = x * NumVertsX + y;
int row2Index = row1Index + NumVertsX;
indices.Write(row1Index);
indices.Write(row1Index + 1);
indices.Write(row2Index + 1);
indices.Write(row1Index);
indices.Write(row2Index + 1);
indices.Write(row2Index);
}
}
}
indexVertexArrays = new TriangleIndexVertexArray();
indexVertexArrays.AddIndexedMesh(mesh);
SetVertexPositions(waveHeight, 0.0f);
const bool useQuantizedAabbCompression = true;
groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
CollisionShapes.Add(groundShape);
staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
staticBody.CollisionFlags |= CollisionFlags.StaticObject;
staticBody.UserObject = "Ground";
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Vector3 worldMin = new Vector3(-1000, -1000, -1000);
Vector3 worldMax = new Vector3(1000, 1000, 1000);
Broadphase = new AxisSweep3(worldMin, worldMax);
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.SolverInfo.SplitImpulse = 1;
World.Gravity = new Vector3(0, -10, 0);
IsDebugDrawEnabled = true;
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
for (int j = 0; j < NumDynamicBoxesX; j++)
{
for (int i = 0; i < NumDynamicBoxesY; i++)
{
//CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
Matrix startTransform = Matrix.Translation(5 * (i - NumDynamicBoxesX / 2), 10, 5 * (j - NumDynamicBoxesY / 2));
LocalCreateRigidBody(1.0f, startTransform, colShape);
}
}
SetVertexPositions(WaveHeight, 0.0f);
const bool useQuantizedAabbCompression = true;
groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
CollisionShapes.Add(groundShape);
staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
staticBody.CollisionFlags |= CollisionFlags.StaticObject;
staticBody.UserObject = "Ground";
}
private RigidBody BuildRigidBodyFromTriangleMeshShape(BvhTriangleMeshShape triangleMeshShape, int mass, TGCVector3 position, TGCVector3 scale)
{
var transformationMatrix = TGCMatrix.RotationYawPitchRoll(0, 0, 0);
transformationMatrix.Origin = position;
DefaultMotionState motionState = new DefaultMotionState(transformationMatrix.ToBsMatrix);
var bulletShape = new ScaledBvhTriangleMeshShape(triangleMeshShape, scale.ToBulletVector3());
var boxLocalInertia = bulletShape.CalculateLocalInertia(0);
var bodyInfo = new RigidBodyConstructionInfo(mass, motionState, bulletShape, boxLocalInertia);
var rigidBody = new RigidBody(bodyInfo);
rigidBody.Friction = 0.4f;
rigidBody.RollingFriction = 1;
rigidBody.Restitution = 1f;
return(rigidBody);
}
public override void SetupFromMesh(Mesh mesh)
{
var triMesh = new TriangleMesh();
int i = 0;
var vertices = mesh.Vertices;
while (i < mesh.Indices.Length)
{
triMesh.AddTriangle(
vertices[mesh.Indices[i++]],
vertices[mesh.Indices[i++]],
vertices[mesh.Indices[i++]]
);
}
collisionShape = new BvhTriangleMeshShape(triMesh, true);
}
private CollisionObject CreateCollisionFromTgcMesh(TgcMesh mesh)
{
var vertexCoords = mesh.getVertexPositions();
TriangleMesh triangleMesh = new TriangleMesh();
for (int i = 0; i < vertexCoords.Length; i = i + 3)
{
triangleMesh.AddTriangle(vertexCoords[i].ToBulletVector3(), vertexCoords[i + 1].ToBulletVector3(), vertexCoords[i + 2].ToBulletVector3());
}
var transformationMatrix = TGCMatrix.RotationYawPitchRoll(0, 0, 0).ToBsMatrix;
var bulletShape = new BvhTriangleMeshShape(triangleMesh, false);
CollisionObject collisionObject = new CollisionObject();
collisionObject.CollisionShape = bulletShape;
return(collisionObject);
}
BvhTriangleMeshShape _CreateBvhTriangleMeshShape()
{
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);
}
BvhTriangleMeshShape ms = new BvhTriangleMeshShape(tm, true);
ms.LocalScaling = m_localScaling.ToBullet();
return(ms);
}
private void CreateGround()
{
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
const int triangleIndexStride = 3 * sizeof(int);
indexVertexArrays = new TriangleIndexVertexArray();
var mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts, triangleIndexStride, Vector3.SizeInBytes, PhyScalarType.Int32, PhyScalarType.Single);
DataStream indices = mesh.LockIndices();
for (int x = 0; x < NumVertsX - 1; x++)
{
for (int y = 0; y < NumVertsY - 1; y++)
{
int row1Index = x * NumVertsX + y;
int row2Index = row1Index + NumVertsX;
indices.Write(row1Index);
indices.Write(row1Index + 1);
indices.Write(row2Index + 1);
indices.Write(row1Index);
indices.Write(row2Index + 1);
indices.Write(row2Index);
}
}
indices.Dispose();
indexVertexArrays = new TriangleIndexVertexArray();
indexVertexArrays.AddIndexedMesh(mesh);
SetVertexPositions(WaveHeight, 0.0f);
const bool useQuantizedAabbCompression = true;
groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
staticBody.CollisionFlags |= CollisionFlags.StaticObject;
staticBody.UserObject = "Ground";
}
public RigidBody AddStaticGeometry(ColladaGeometry geometry, Matrix4 transform, object UserData)
{
TriangleMesh mesh = new TriangleMesh();
foreach (Triangle tri in geometry.triangles)
{
mesh.AddTriangle(tri.vertices[0], tri.vertices[1], tri.vertices[2]
);
}
CollisionShape shape = new BvhTriangleMeshShape(mesh, true);
shape.UserObject = UserData;
collisionShapes.Add(shape);
RigidBody body = CreateRigidBody(0, transform, shape); //Zero mass for static body
return(body);
}
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 BvhTriangleMeshShape(tm, true);
}
return(collisionShapePtr);
}
private static CollisionShape loadConcaveMesh(Mesh mesh, Vector3 scale)
{
TriangleMesh trimesh = new TriangleMesh();
for (int i = 0; i < mesh.submeshes[0].vertex_data.Length; i += 3)
{
int index0 = (int)mesh.submeshes[0].index_data[i];
int index1 = (int)mesh.submeshes[0].index_data[i + 1];
int index2 = (int)mesh.submeshes[0].index_data[i + 2];
Vector3 vertex0 = new Vector3(mesh.submeshes[0].vertex_data[index0].position.X, mesh.submeshes[0].vertex_data[index0].position.Y, mesh.submeshes[0].vertex_data[index0].position.Z) * (scale);
Vector3 vertex1 = new Vector3(mesh.submeshes[0].vertex_data[index1].position.X, mesh.submeshes[0].vertex_data[index1].position.Y, mesh.submeshes[0].vertex_data[index1].position.Z) * (scale);
Vector3 vertex2 = new Vector3(mesh.submeshes[0].vertex_data[index2].position.X, mesh.submeshes[0].vertex_data[index2].position.Y, mesh.submeshes[0].vertex_data[index2].position.Z) * (scale);
trimesh.AddTriangle(vertex0, vertex1, vertex2);
}
CollisionShape concaveShape = new BvhTriangleMeshShape(trimesh, true, true);
return(concaveShape);
}
public BulletTriangleMesh(IModelo model, Vector3 pos, Matrix rotation, Vector3 scale)
{
TriangleMesh TriangleMesh = new BulletSharp.TriangleMesh(true, false);
Vector3[] vertices = null;
int[] indices = null;
ExtractData(ref vertices, ref indices, model);
for (int i = 0; i < indices.Count(); i += 3)
{
TriangleMesh.AddTriangle(vertices[indices[i]], vertices[indices[i + 1]], vertices[indices[i + 2]]);
}
vertices = null;
indices = null;
BvhTriangleMeshShape = new BvhTriangleMeshShape(TriangleMesh, true, true);
this.Shape = BvhTriangleMeshShape;
this.Scale = scale;
Shape.LocalScaling = scale;
Object = LocalCreateRigidBody(0, Matrix.CreateTranslation(pos) * rotation, Shape);
}
public MeshCollider(GameObject obj)
{
gameObject = obj;
MeshIndices.AddRange(gameObject.GetMeshRender()._mesh._indices);
for (int i = 0; i < gameObject.GetMeshRender()._mesh._vertices.Length; i += 3)
{
Vector3 pos = new Vector3(gameObject.GetMeshRender()._mesh._vertices[i], gameObject.GetMeshRender()._mesh._vertices[i + 1], gameObject.GetMeshRender()._mesh._vertices[i + 2]);
MeshVertices.Add(pos);
}
//ANTIGO ANTIGO ANTIGO
// Initialize TriangleIndexVertexArray with float array
/*indexVertexArray = new TriangleIndexVertexArray(MeshIndices.ToArray(), MeshVertices.ToArray());
* gImpactMeshShape = new GImpactMeshShape(indexVertexArray);
* gImpactMeshShape.CalculateLocalInertia(1.0f);
* gImpactMesh = CreateBody(1.0f, gImpactMeshShape, Vector3.Zero);*/
indexVertexArray2 = new TriangleIndexVertexArray(MeshIndices.ToArray(), MeshVertices.ToArray());
triangleMeshShape = new BvhTriangleMeshShape(indexVertexArray2, true);
_collisionObject = new CollisionObject();
_collisionObject.WorldTransform = gameObject._transform.RotationMatrix * gameObject._transform.PositionMatrix * Matrix4.CreateScale(gameObject._transform.Size);
_collisionObject.CollisionShape = triangleMeshShape;
//TestTriangleArray(indexVertexArray);
//TestTriangleArray(indexVertexArray2);
Vector3 aabbMin, aabbMax;
//gImpactMeshShape.GetAabb(Matrix4.Identity, out aabbMin, out aabbMax);
triangleMeshShape.GetAabb(Matrix4.Identity, out aabbMin, out aabbMax);
Physics.AddCollisionObject(_collisionObject);
}
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
public static void GenerateInternalEdgeInfo(BvhTriangleMeshShape trimeshShape, TriangleInfoMap triangleInfoMap)
{
//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
if (trimeshShape.GetTriangleInfoMap() != null)
{
return;
}
trimeshShape.SetTriangleInfoMap(triangleInfoMap);
StridingMeshInterface meshInterface = trimeshShape.GetMeshInterface();
IndexedVector3 meshScaling = meshInterface.GetScaling();
for (int partId = 0; partId < meshInterface.GetNumSubParts(); partId++)
{
object vertexbase = null;
int numverts = 0;
PHY_ScalarType type = PHY_ScalarType.PHY_INTEGER;
int stride = 0;
object indexbase = null;
int indexstride = 0;
int numfaces = 0;
PHY_ScalarType indicestype = PHY_ScalarType.PHY_INTEGER;
//PHY_ScalarType indexType=0;
IndexedVector3[] triangleVerts = new IndexedVector3[3];
meshInterface.GetLockedReadOnlyVertexIndexBase(out vertexbase, out numverts, out type, out stride, out indexbase, out indexstride, out numfaces, out indicestype, partId);
IndexedVector3 aabbMin, aabbMax;
switch (indicestype)
{
case PHY_ScalarType.PHY_INTEGER:
{
int[] indexList = ((ObjectArray<int>)indexbase).GetRawArray();
if (vertexbase is ObjectArray<IndexedVector3>)
{
IndexedVector3[] vertexList = (vertexbase as ObjectArray<IndexedVector3>).GetRawArray();
int indexCounter = 0;
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
int index1 = indexList[triangleIndex];
int index2 = indexList[triangleIndex + 1];
int index3 = indexList[triangleIndex + 2];
triangleVerts[0] = new IndexedVector3(vertexList[index1]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[index2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[index3]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
#if XNA
else if (vertexbase is ObjectArray<Microsoft.Xna.Framework.Vector3>)
{
Microsoft.Xna.Framework.Vector3[] vertexList = (vertexbase as ObjectArray<Microsoft.Xna.Framework.Vector3>).GetRawArray();
int indexCounter = 0;
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
int index1 = indexList[triangleIndex];
int index2 = indexList[triangleIndex + 1];
int index3 = indexList[triangleIndex + 2];
triangleVerts[0] = new IndexedVector3(vertexList[index1]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[index2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[index3]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
#endif
else if (vertexbase is ObjectArray<float>)
{
float[] vertexList = (vertexbase as ObjectArray<float>).GetRawArray();
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
triangleVerts[0] = new IndexedVector3(vertexList[indexList[triangleIndex]], vertexList[indexList[triangleIndex] + 1], vertexList[indexList[triangleIndex] + 2]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[indexList[triangleIndex + 1]], vertexList[indexList[triangleIndex + 1] + 1], vertexList[indexList[triangleIndex + 1] + 2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[indexList[triangleIndex + 2]], vertexList[indexList[triangleIndex + 2] + 1], vertexList[indexList[triangleIndex + 2] + 2]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
break;
}
default:
{
Debug.Assert(indicestype == PHY_ScalarType.PHY_INTEGER);
break;
}
}
}
}
private static void ProcessResult(IndexedVector3[] triangleVerts, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax, BvhTriangleMeshShape trimeshShape, int partId, int triangleIndex, TriangleInfoMap triangleInfoMap)
{
aabbMin = MathUtil.MAX_VECTOR;
aabbMax = MathUtil.MIN_VECTOR;
aabbMin.SetMin(ref triangleVerts[0]);
aabbMax.SetMax(ref triangleVerts[0]);
aabbMin.SetMin(ref triangleVerts[1]);
aabbMax.SetMax(ref triangleVerts[1]);
aabbMin.SetMin(ref triangleVerts[2]);
aabbMax.SetMax(ref triangleVerts[2]);
ConnectivityProcessor connectivityProcessor = new ConnectivityProcessor();
connectivityProcessor.m_partIdA = partId;
connectivityProcessor.m_triangleIndexA = triangleIndex;
connectivityProcessor.m_triangleVerticesA = triangleVerts;
connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
trimeshShape.ProcessAllTriangles(connectivityProcessor, ref aabbMin, ref aabbMax);
}