protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
//Broadphase = new SimpleBroadphase();
Broadphase = new AxisSweep3_32Bit(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000), 1024);
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create trimesh model and shape
InitGImpactCollision();
// Create Scene
float mass = 0.0f;
CollisionShape staticboxShape1 = new BoxShape(200, 1, 200);//floor
CollisionShapes.Add(staticboxShape1);
LocalCreateRigidBody(mass, Matrix.Translation(0, -10, 0), staticboxShape1);
CollisionShape staticboxShape2 = new BoxShape(1, 50, 200);//left wall
CollisionShapes.Add(staticboxShape2);
LocalCreateRigidBody(mass, Matrix.Translation(-200, 15, 0), staticboxShape2);
CollisionShape staticboxShape3 = new BoxShape(1, 50, 200);//right wall
CollisionShapes.Add(staticboxShape3);
LocalCreateRigidBody(mass, Matrix.Translation(200, 15, 0), staticboxShape3);
CollisionShape staticboxShape4 = new BoxShape(200, 50, 1);//front wall
CollisionShapes.Add(staticboxShape4);
LocalCreateRigidBody(mass, Matrix.Translation(0, 15, 200), staticboxShape4);
CollisionShape staticboxShape5 = new BoxShape(200, 50, 1);//back wall
CollisionShapes.Add(staticboxShape5);
LocalCreateRigidBody(mass, Matrix.Translation(0, 15, -200), staticboxShape5);
//static plane
Vector3 normal = new Vector3(-0.5f, 0.5f, 0.0f);
normal.Normalize();
CollisionShape staticplaneShape6 = new StaticPlaneShape(normal, 0.5f);// A plane
CollisionShapes.Add(staticplaneShape6);
/*RigidBody staticBody2 =*/ LocalCreateRigidBody(mass, Matrix.Translation(0, -9, 0), staticplaneShape6);
//another static plane
normal = new Vector3(0.5f, 0.7f, 0.0f);
//normal.Normalize();
CollisionShape staticplaneShape7 = new StaticPlaneShape(normal, 0.0f);// A plane
CollisionShapes.Add(staticplaneShape7);
/*staticBody2 =*/ LocalCreateRigidBody(mass, Matrix.Translation(0, -10, 0), staticplaneShape7);
// Create Static Torus
float height = 28;
const float step = 2.5f;
const float massT = 1.0f;
Matrix startTransform =
Matrix.RotationQuaternion(Quaternion.RotationYawPitchRoll((float)Math.PI * 0.5f, 0, (float)Math.PI * 0.5f)) *
Matrix.Translation(0, height, -5);
#if BULLET_GIMPACT
kinematicTorus = LocalCreateRigidBody(0, startTransform, trimeshShape);
#else
//kinematicTorus = LocalCreateRigidBody(0, startTransform, CreateTorusShape());
#endif
//kinematicTorus.CollisionFlags = kinematicTorus.CollisionFlags | CollisionFlags.StaticObject;
//kinematicTorus.ActivationState = ActivationState.IslandSleeping;
kinematicTorus.CollisionFlags = kinematicTorus.CollisionFlags | CollisionFlags.KinematicObject;
kinematicTorus.ActivationState = ActivationState.DisableDeactivation;
// Kinematic
//kinTorusTran = new Vector3(-0.1f, 0, 0);
//kinTorusRot = Quaternion.RotationYawPitchRoll(0, (float)Math.PI * 0.01f, 0);
#if TEST_GIMPACT_TORUS
#if BULLET_GIMPACT
// Create dynamic Torus
for (int i = 0; i < 6; i++)
{
height -= step;
startTransform =
Matrix.RotationQuaternion(Quaternion.RotationYawPitchRoll(0, 0, (float)Math.PI * 0.5f)) *
Matrix.Translation(0, height, -5);
/*RigidBody bodyA =*/ LocalCreateRigidBody(massT, startTransform, trimeshShape);
height -= step;
startTransform =
Matrix.RotationQuaternion(Quaternion.RotationYawPitchRoll((float)Math.PI * 0.5f, 0, (float)Math.PI * 0.5f)) *
Matrix.Translation(0, height, -5);
/*RigidBody bodyB =*/ LocalCreateRigidBody(massT, startTransform, trimeshShape);
}
#else
/*
* // Create dynamic Torus
* for (int i = 0; i < 6; i++)
* {
* height -= step;
* startTransform.setOrigin(btVector3(0, height, -5));
* startTransform.setRotation(btQuaternion(0, 0, 3.14159265 * 0.5));
*
* btRigidBody* bodyA = localCreateRigidBody(massT, startTransform, createTorusShape());
*
* height -= step;
* startTransform.setOrigin(btVector3(0, height, -5));
* startTransform.setRotation(btQuaternion(3.14159265 * 0.5, 0, 3.14159265 * 0.5));
* btRigidBody* bodyB = localCreateRigidBody(massT, startTransform, createTorusShape());
* }
*/
#endif
#endif
// Create Dynamic Boxes
for (int i = 0; i < 8; i++)
{
CollisionShape boxShape = new BoxShape(new Vector3(1, 1, 1));
CollisionShapes.Add(boxShape);
LocalCreateRigidBody(1, Matrix.Translation(2 * i - 5, 2, -3), boxShape);
}
}
protected override void OnInitializePhysics()
{
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
const int NumVertsX = 20;
const int NumVertsY = 20;
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts);
mesh.NumTriangles = totalTriangles;
mesh.NumVertices = totalVerts;
mesh.TriangleIndexStride = 3 * sizeof(int);
mesh.VertexStride = Vector3.SizeInBytes;
using (var indicesStream = mesh.GetTriangleStream())
{
var indices = new BinaryWriter(indicesStream);
for (i = 0; i < NumVertsX - 1; i++)
{
for (int j = 0; j < NumVertsY - 1; j++)
{
indices.Write(j * NumVertsX + i);
indices.Write(j * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write(j * NumVertsX + i);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i);
}
}
indices.Dispose();
}
using (var vertexStream = mesh.GetVertexStream())
{
var vertices = new BinaryWriter(vertexStream);
for (i = 0; i < NumVertsX; i++)
{
for (int j = 0; j < NumVertsY; j++)
{
float wl = .2f;
float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));
vertices.Write((i - NumVertsX * 0.5f) * scale);
vertices.Write(height);
vertices.Write((j - NumVertsY * 0.5f) * scale);
}
}
vertices.Dispose();
}
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.PhyFloat;
* 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.PhyFloat)
* {
* // heightScale isn't applied internally for float data
* height = reader.ReadSingle();
* }
* else if (scalarType == PhyScalarType.PhyUChar)
* {
* 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
VehicleTuning tuning = new VehicleTuning();
IVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(World);
//vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
vehicle = new CustomVehicle(tuning, carChassis, vehicleRayCaster);
carChassis.ActivationState = ActivationState.DisableDeactivation;
World.AddAction(vehicle);
const float connectionHeight = 1.2f;
bool isFrontWheel = true;
// choose coordinate system
vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
BulletSharp.Math.Vector3 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);
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;
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
// Use the default collision dispatcher. For parallel processing you can use a diffent dispatcher.
Dispatcher = new CollisionDispatcher(CollisionConf);
VoronoiSimplexSolver simplex = new VoronoiSimplexSolver();
MinkowskiPenetrationDepthSolver pdSolver = new MinkowskiPenetrationDepthSolver();
Convex2DConvex2DAlgorithm.CreateFunc convexAlgo2D = new Convex2DConvex2DAlgorithm.CreateFunc(simplex, pdSolver);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Convex2DShape, BroadphaseNativeType.Convex2DShape, convexAlgo2D);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Box2DShape, BroadphaseNativeType.Convex2DShape, convexAlgo2D);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Convex2DShape, BroadphaseNativeType.Box2DShape, convexAlgo2D);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Box2DShape, BroadphaseNativeType.Box2DShape, new Box2DBox2DCollisionAlgorithm.CreateFunc());
Broadphase = new DbvtBroadphase();
// the default constraint solver.
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create a few basic rigid bodies
CollisionShape groundShape = new BoxShape(150, 7, 150);
CollisionShapes.Add(groundShape);
RigidBody ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
float u = 0.96f;
Vector3[] points = { new Vector3(0, u, 0), new Vector3(-u, -u, 0), new Vector3(u, -u, 0) };
ConvexShape childShape0 = new BoxShape(1, 1, Depth);
ConvexShape colShape = new Convex2DShape(childShape0);
ConvexShape childShape1 = new ConvexHullShape(points);
ConvexShape colShape2 = new Convex2DShape(childShape1);
ConvexShape childShape2 = new CylinderShapeZ(1, 1, Depth);
ConvexShape colShape3 = new Convex2DShape(childShape2);
CollisionShapes.Add(colShape);
CollisionShapes.Add(colShape2);
CollisionShapes.Add(colShape3);
CollisionShapes.Add(childShape0);
CollisionShapes.Add(childShape1);
CollisionShapes.Add(childShape2);
colShape.Margin = 0.03f;
float mass = 1.0f;
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
Matrix startTransform;
Vector3 x = new Vector3(-ArraySizeX, 8, -20);
Vector3 y = Vector3.Zero;
Vector3 deltaX = new Vector3(1, 2, 0);
Vector3 deltaY = new Vector3(2, 0, 0);
var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
int i, j;
for (i = 0; i < ArraySizeY; i++)
{
y = x;
for (j = 0; j < ArraySizeX; j++)
{
startTransform = Matrix.Translation(y + new Vector3(10, 0, 0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
switch (j % 3)
{
case 0:
rbInfo.CollisionShape = colShape;
break;
case 1:
rbInfo.CollisionShape = colShape3;
break;
default:
rbInfo.CollisionShape = colShape2;
break;
}
RigidBody body = new RigidBody(rbInfo);
//body.ActivationState = ActivationState.IslandSleeping;
body.LinearFactor = new Vector3(1, 1, 0);
body.AngularFactor = new Vector3(0, 0, 1);
World.AddRigidBody(body);
y += deltaY;
}
x += deltaX;
}
rbInfo.Dispose();
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
//CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
convexDecompositionObjectOffset = new Vector3(10, 0, 0);
// Load wavefront file
var wo = new WavefrontObj();
//string filename = UnityEngine.Application.dataPath + "/BulletUnity/Examples/Scripts/BulletSharpDemos/ConvexDecompositionDemo/data/file.obj";
UnityEngine.TextAsset bytes = (UnityEngine.TextAsset)UnityEngine.Resources.Load("file.obj");
System.IO.Stream byteStream = new System.IO.MemoryStream(bytes.bytes);
int tcount = wo.LoadObj(byteStream);
if (tcount == 0)
{
return;
}
// Convert file data to TriangleMesh
var trimesh = new TriangleMesh();
trimeshes.Add(trimesh);
Vector3 localScaling = new Vector3(6, 6, 6);
List <int> indices = wo.Indices;
List <Vector3> vertices = wo.Vertices;
int i;
for (i = 0; i < tcount; i++)
{
int index0 = indices[i * 3];
int index1 = indices[i * 3 + 1];
int index2 = indices[i * 3 + 2];
Vector3 vertex0 = vertices[index0] * localScaling;
Vector3 vertex1 = vertices[index1] * localScaling;
Vector3 vertex2 = vertices[index2] * localScaling;
trimesh.AddTriangleRef(ref vertex0, ref vertex1, ref vertex2);
}
// Create a hull approximation
ConvexHullShape convexShape;
using (var tmpConvexShape = new ConvexTriangleMeshShape(trimesh))
{
using (var hull = new ShapeHull(tmpConvexShape))
{
hull.BuildHull(tmpConvexShape.Margin);
convexShape = new ConvexHullShape(hull.Vertices);
}
}
if (sEnableSAT)
{
convexShape.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape);
// Add non-moving body to world
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
CollisionShapes.Add(concaveShape);
// HACD
var hacd = new Hacd();
hacd.SetPoints(wo.Vertices);
hacd.SetTriangles(wo.Indices);
hacd.CompacityWeight = 0.1;
hacd.VolumeWeight = 0.0;
// Recommended HACD parameters: 2 100 false false false
hacd.NClusters = 2; // minimum number of clusters
hacd.Concavity = 100; // maximum concavity
hacd.AddExtraDistPoints = false;
hacd.AddNeighboursDistPoints = false;
hacd.AddFacesPoints = false;
hacd.NVerticesPerCH = 100; // max of 100 vertices per convex-hull
hacd.Compute();
hacd.Save("output.wrl", false);
// Generate convex result
var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
var writer = new StreamWriter(outputFile);
var convexDecomposition = new ConvexDecomposition(writer, this);
convexDecomposition.LocalScaling = localScaling;
for (int c = 0; c < hacd.NClusters; c++)
{
int nVertices = hacd.GetNPointsCH(c);
int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
double[] points = new double[nVertices * 3];
long[] triangles = new long[trianglesLen];
hacd.GetCH(c, points, triangles);
if (trianglesLen == 0)
{
continue;
}
Vector3[] verticesArray = new Vector3[nVertices];
int vi3 = 0;
for (int vi = 0; vi < nVertices; vi++)
{
verticesArray[vi] = new Vector3(
(float)points[vi3], (float)points[vi3 + 1], (float)points[vi3 + 2]);
vi3 += 3;
}
int[] trianglesInt = new int[trianglesLen];
for (int ti = 0; ti < trianglesLen; ti++)
{
trianglesInt[ti] = (int)triangles[ti];
}
convexDecomposition.ConvexDecompResult(verticesArray, trianglesInt);
}
// Combine convex shapes into a compound shape
var compound = new CompoundShape();
for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
var convexShape2 = convexDecomposition.convexShapes[i];
Matrix trans = Matrix.Translation(centroid);
if (sEnableSAT)
{
convexShape2.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape2);
compound.AddChildShape(trans, convexShape2);
LocalCreateRigidBody(1.0f, trans, convexShape2);
}
CollisionShapes.Add(compound);
writer.Dispose();
outputFile.Dispose();
#if true
mass = 10.0f;
var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
/*
* ThreadSupportInterface CreateSolverThreadSupport(int maxNumThreads)
* {
* //#define SEQUENTIAL
* /* if (SEQUENTIAL)
* {
* SequentialThreadSupport::SequentialThreadConstructionInfo tci("solverThreads",SolverThreadFunc,SolverlsMemoryFunc);
* SequentialThreadSupport* threadSupport = new SequentialThreadSupport(tci);
* threadSupport->startSPU();
* }
* else * /
*
* Win32ThreadConstructionInfo threadConstructionInfo = new Win32ThreadConstructionInfo("solverThreads",
* Win32ThreadFunc.SolverThreadFunc, Win32LSMemorySetupFunc.SolverLSMemoryFunc, maxNumThreads);
* Win32ThreadSupport threadSupport = new Win32ThreadSupport(threadConstructionInfo);
* threadSupport.StartSpu();
* return threadSupport;
* }
*/
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
DefaultCollisionConstructionInfo cci = new DefaultCollisionConstructionInfo();
cci.DefaultMaxPersistentManifoldPoolSize = 32768;
CollisionConf = new DefaultCollisionConfiguration(cci);
if (UseParallelDispatcherBenchmark)
{
//int maxNumOutstandingTasks = 4;
/*Win32ThreadConstructionInfo info = new Win32ThreadConstructionInfo("collision",
* Win32ThreadFunc.ProcessCollisionTask, Win32LSMemorySetupFunc.CreateCollisionLocalStoreMemory,
* maxNumOutstandingTasks);
*
* Win32ThreadSupport threadSupportCollision = new Win32ThreadSupport(info);
* Dispatcher = new SpuGatheringCollisionDispatcher(threadSupportCollision, 1, CollisionConf);*/
}
else
{
Dispatcher = new CollisionDispatcher(CollisionConf);
Dispatcher.DispatcherFlags = DispatcherFlags.DisableContactPoolDynamicAllocation;
}
// the maximum size of the collision world. Make sure objects stay within these boundaries
// Don't make the world AABB size too large, it will harm simulation quality and performance
Vector3 worldAabbMin = new Vector3(-1000, -1000, -1000);
Vector3 worldAabbMax = new Vector3(1000, 1000, 1000);
HashedOverlappingPairCache pairCache = new HashedOverlappingPairCache();
Broadphase = new AxisSweep3(worldAabbMin, worldAabbMax, 3500, pairCache);
//Broadphase = new DbvtBroadphase();
if (UseParallelDispatcherBenchmark)
{
//ThreadSupportInterface thread = CreateSolverThreadSupport(4);
//Solver = new ParallelConstraintSolver(thread);
}
else
{
Solver = new SequentialImpulseConstraintSolver();
}
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
if (UseParallelDispatcherBenchmark)
{
((DiscreteDynamicsWorld)World).SimulationIslandManager.SplitIslands = false;
}
World.SolverInfo.SolverMode |= SolverModes.EnableFrictionDirectionCaching;
World.SolverInfo.NumIterations = 5;
if (benchmark < 5)
{
// create the ground
CollisionShape groundShape = new BoxShape(250, 50, 250);
CollisionShapes.Add(groundShape);
CollisionObject ground = base.LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
}
float cubeSize = 1.0f;
float spacing = cubeSize;
float mass = 1.0f;
int size = 8;
Vector3 pos = new Vector3(0.0f, cubeSize * 2, 0.0f);
float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
switch (benchmark)
{
case 1:
// 3000
BoxShape blockShape = new BoxShape(cubeSize - collisionRadius);
mass = 2.0f;
for (int k = 0; k < 47; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
RigidBody cmbody = LocalCreateRigidBody(mass, Matrix.Translation(pos), blockShape);
}
}
offset -= 0.05f * spacing * (size - 1);
// spacing *= 1.01f;
pos[1] += (cubeSize * 2.0f + spacing);
}
break;
case 2:
CreatePyramid(new Vector3(-20, 0, 0), 12, new Vector3(cubeSize));
CreateWall(new Vector3(-2.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
CreateWall(new Vector3(4.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
CreateWall(new Vector3(10.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
CreateTowerCircle(new Vector3(25.0f, 0.0f, 0.0f), 8, 24, new Vector3(cubeSize));
break;
case 3:
// TODO: Ragdolls
break;
case 4:
cubeSize = 1.5f;
ConvexHullShape convexHullShape = new ConvexHullShape();
float scaling = 1;
convexHullShape.LocalScaling = new Vector3(scaling);
for (int i = 0; i < Taru.Vtx.Length / 3; i++)
{
Vector3 vtx = new Vector3(Taru.Vtx[i * 3], Taru.Vtx[i * 3 + 1], Taru.Vtx[i * 3 + 2]);
convexHullShape.AddPoint(vtx * (1.0f / scaling));
}
//this will enable polyhedral contact clipping, better quality, slightly slower
convexHullShape.InitializePolyhedralFeatures();
for (int k = 0; k < 15; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
LocalCreateRigidBody(mass, Matrix.Translation(pos), convexHullShape);
}
}
offset -= 0.05f * spacing * (size - 1);
spacing *= 1.01f;
pos[1] += (cubeSize * 2.0f + spacing);
}
break;
case 5:
Vector3 boxSize = new Vector3(1.5f);
float boxMass = 1.0f;
float sphereRadius = 1.5f;
float sphereMass = 1.0f;
float capsuleHalf = 2.0f;
float capsuleRadius = 1.0f;
float capsuleMass = 1.0f;
size = 10;
int height = 10;
cubeSize = boxSize[0];
spacing = 2.0f;
pos = new Vector3(0.0f, 20.0f, 0.0f);
offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
int numBodies = 0;
Random random = new Random();
for (int k = 0; k < height; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
Vector3 bpos = new Vector3(0, 25, 0) + new Vector3(5.0f * pos.X, pos.Y, 5.0f * pos.Z);
int idx = random.Next(10);
Matrix trans = Matrix.Translation(bpos);
switch (idx)
{
case 0:
case 1:
case 2:
{
float r = 0.5f * (idx + 1);
BoxShape boxShape = new BoxShape(boxSize * r);
LocalCreateRigidBody(boxMass * r, trans, boxShape);
}
break;
case 3:
case 4:
case 5:
{
float r = 0.5f * (idx - 3 + 1);
SphereShape sphereShape = new SphereShape(sphereRadius * r);
LocalCreateRigidBody(sphereMass * r, trans, sphereShape);
}
break;
case 6:
case 7:
case 8:
{
float r = 0.5f * (idx - 6 + 1);
CapsuleShape capsuleShape = new CapsuleShape(capsuleRadius * r, capsuleHalf * r);
LocalCreateRigidBody(capsuleMass * r, trans, capsuleShape);
}
break;
}
numBodies++;
}
}
offset -= 0.05f * spacing * (size - 1);
spacing *= 1.1f;
pos[1] += (cubeSize * 2.0f + spacing);
}
//CreateLargeMeshBody();
break;
case 6:
boxSize = new Vector3(1.5f, 1.5f, 1.5f);
convexHullShape = new ConvexHullShape();
for (int i = 0; i < Taru.Vtx.Length / 3; i++)
{
Vector3 vtx = new Vector3(Taru.Vtx[i * 3], Taru.Vtx[i * 3 + 1], Taru.Vtx[i * 3 + 2]);
convexHullShape.AddPoint(vtx);
}
size = 10;
height = 10;
cubeSize = boxSize[0];
spacing = 2.0f;
pos = new Vector3(0.0f, 20.0f, 0.0f);
offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
for (int k = 0; k < height; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
Vector3 bpos = new Vector3(0, 25, 0) + new Vector3(5.0f * pos.X, pos.Y, 5.0f * pos.Z);
LocalCreateRigidBody(mass, Matrix.Translation(bpos), convexHullShape);
}
}
offset -= 0.05f * spacing * (size - 1);
spacing *= 1.1f;
pos[1] += (cubeSize * 2.0f + spacing);
}
//CreateLargeMeshBody();
break;
case 7:
// TODO
//CreateTest6();
//InitRays();
break;
}
}
public override void InitPhysics()
{
SetupEmptyDynamicsWorld();
WavefrontObj wo = new WavefrontObj();
int tcount = wo.LoadObj("file.obj");
if (tcount > 0)
{
TriangleMesh trimesh = new TriangleMesh();
trimeshes.Add(trimesh);
Vector3 localScaling = new Vector3(6, 6, 6);
List <int> indices = wo.Indices;
List <Vector3> vertices = wo.Vertices;
int i;
for (i = 0; i < tcount; i++)
{
int index0 = indices[i * 3];
int index1 = indices[i * 3 + 1];
int index2 = indices[i * 3 + 2];
Vector3 vertex0 = Vector3.Modulate(vertices[index0], localScaling);
Vector3 vertex1 = Vector3.Modulate(vertices[index1], localScaling);
Vector3 vertex2 = Vector3.Modulate(vertices[index2], localScaling);
trimesh.AddTriangle(vertex0, vertex1, vertex2);
}
ConvexShape tmpConvexShape = new ConvexTriangleMeshShape(trimesh);
//create a hull approximation
ShapeHull hull = new ShapeHull(tmpConvexShape);
float margin = tmpConvexShape.Margin;
hull.BuildHull(margin);
tmpConvexShape.UserObject = hull;
ConvexHullShape convexShape = new ConvexHullShape();
foreach (Vector3 v in hull.Vertices)
{
convexShape.AddPoint(v);
}
tmpConvexShape.Dispose();
//hull.Dispose();
CollisionShapes.Add(convexShape);
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
bool useQuantization = true;
CollisionShape concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
CollisionShapes.Add(concaveShape);
// Bullet Convex Decomposition
FileStream outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
StreamWriter writer = new StreamWriter(outputFile);
DecompDesc desc = new DecompDesc();
desc.mVertices = wo.Vertices.ToArray();
desc.mTcount = tcount;
desc.mIndices = wo.Indices.ToArray();
desc.mDepth = 5;
desc.mCpercent = 5;
desc.mPpercent = 15;
desc.mMaxVertices = 16;
desc.mSkinWidth = 0.0f;
MyConvexDecomposition convexDecomposition = new MyConvexDecomposition(writer, this);
desc.mCallback = convexDecomposition;
// HACD
Hacd myHACD = new Hacd();
myHACD.SetPoints(wo.Vertices);
myHACD.SetTriangles(wo.Indices);
myHACD.CompacityWeight = 0.1;
myHACD.VolumeWeight = 0.0;
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
int nClusters = 2;
double concavity = 100;
//bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
myHACD.NClusters = nClusters; // minimum number of clusters
myHACD.VerticesPerConvexHull = 100; // max of 100 vertices per convex-hull
myHACD.Concavity = concavity; // maximum concavity
myHACD.AddExtraDistPoints = addExtraDistPoints;
myHACD.AddNeighboursDistPoints = addNeighboursDistPoints;
myHACD.AddFacesPoints = addFacesPoints;
myHACD.Compute();
nClusters = myHACD.NClusters;
myHACD.Save("output.wrl", false);
if (true)
{
CompoundShape compound = new CompoundShape();
CollisionShapes.Add(compound);
Matrix trans = Matrix.Identity;
for (int c = 0; c < nClusters; c++)
{
//generate convex result
Vector3[] points;
int[] triangles;
myHACD.GetCH(c, out points, out triangles);
ConvexResult r = new ConvexResult(points, triangles);
convexDecomposition.ConvexDecompResult(r);
}
for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
trans = Matrix.Translation(centroid);
ConvexHullShape convexShape2 = convexDecomposition.convexShapes[i] as ConvexHullShape;
compound.AddChildShape(trans, convexShape2);
RigidBody body = LocalCreateRigidBody(1.0f, trans, convexShape2);
}
#if true
mass = 10.0f;
trans = Matrix.Translation(-convexDecompositionObjectOffset);
RigidBody body2 = LocalCreateRigidBody(mass, trans, compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
trans = Matrix.Translation(-convexDecompositionObjectOffset);
body2 = LocalCreateRigidBody(mass, trans, compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
trans = Matrix.Translation(-convexDecompositionObjectOffset);
body2 = LocalCreateRigidBody(mass, trans, compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
if (outputFile != null)
{
if (writer != null)
{
writer.Dispose();
}
outputFile.Dispose();
}
}
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present
/// in the entry, this filter will be passed into inner ray casts.</param>
/// <param name="rayHit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit rayHit)
{
CompoundChild hitChild;
bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, filter, out rayHit, out hitChild);
return hit;
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="result">Data and hit object from the first impact, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref System.Numerics.Vector3 sweep, out RayCastResult result)
{
CompoundChild hitChild;
RayHit rayHit;
bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, out rayHit, out hitChild);
result = new RayCastResult { HitData = rayHit, HitObject = hitChild.CollisionInformation };
return hit;
}
public Physics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
float mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
if (P2P)
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0,body1,pivotInA,pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0,body1,pivotInA,pivotInB,axisInA,axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
else
{
HingeConstraint hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
float targetVelocity = 1.0f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
float angle = 0;//SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA);
spSlider6Dof.LinearLowerLimit = lowerSliderLimit;
spSlider6Dof.LinearUpperLimit = hiSliderLimit;
//range should be small, otherwise singularities will 'explode' the constraint
//spSlider6Dof.AngularLowerLimit = new Vector3(-1.5f,0,0);
//spSlider6Dof.AngularUpperLimit = new Vector3(1.5f,0,0);
//spSlider6Dof.AngularLowerLimit = new Vector3(0,0,0);
//spSlider6Dof.AngularUpperLimit = new Vector3(0,0,0);
spSlider6Dof.AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0);
spSlider6Dof.AngularUpperLimit = new Vector3(1.5f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 PivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, btPivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
pGen6DOFSpring.LinearUpperLimit = new Vector3(5, 0, 0);
pGen6DOFSpring.LinearLowerLimit = new Vector3(-5, 0, 0);
pGen6DOFSpring.AngularLowerLimit = new Vector3(0, 0, -1.5f);
pGen6DOFSpring.AngularUpperLimit = new Vector3(0, 0, 1.5f);
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.DebugDrawSize = 5;
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
Vector3 pivotA = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
private void Create2dBodies()
{
// Re-using the same collision is better for memory usage and performance
float u = 0.96f;
Vector3[] points = { new Vector3(0, u, 0), new Vector3(-u, -u, 0), new Vector3(u, -u, 0) };
var childShape0 = new BoxShape(1, 1, Depth);
var colShape = new Convex2DShape(childShape0);
var childShape1 = new ConvexHullShape(points);
var colShape2 = new Convex2DShape(childShape1);
var childShape2 = new CylinderShapeZ(1, 1, Depth);
var colShape3 = new Convex2DShape(childShape2);
CollisionShapes.Add(colShape);
CollisionShapes.Add(colShape2);
CollisionShapes.Add(colShape3);
CollisionShapes.Add(childShape0);
CollisionShapes.Add(childShape1);
CollisionShapes.Add(childShape2);
colShape.Margin = 0.03f;
float mass = 1.0f;
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
Vector3 x = new Vector3(-ArraySizeX, 8, -20);
Vector3 y = Vector3.Zero;
Vector3 deltaX = new Vector3(1, 2, 0);
Vector3 deltaY = new Vector3(2, 0, 0);
for (int i = 0; i < ArraySizeY; i++)
{
y = x;
for (int j = 0; j < ArraySizeX; j++)
{
Matrix startTransform = Matrix.Translation(y - new Vector3(-10, 0, 0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
switch (j % 3)
{
case 0:
rbInfo.CollisionShape = colShape;
break;
case 1:
rbInfo.CollisionShape = colShape3;
break;
default:
rbInfo.CollisionShape = colShape2;
break;
}
var body = new RigidBody(rbInfo)
{
//ActivationState = ActivationState.IslandSleeping,
LinearFactor = new Vector3(1, 1, 0),
AngularFactor = new Vector3(0, 0, 1)
};
World.AddRigidBody(body);
y += deltaY;
}
x += deltaX;
}
rbInfo.Dispose();
}
protected override void OnInitializePhysics()
{
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
const int NumVertsX = 20;
const int NumVertsY = 20;
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts);
mesh.NumTriangles = totalTriangles;
mesh.NumVertices = totalVerts;
mesh.TriangleIndexStride = 3 * sizeof(int);
mesh.VertexStride = Vector3.SizeInBytes;
using (var indicesStream = mesh.GetTriangleStream())
{
var indices = new BinaryWriter(indicesStream);
for (i = 0; i < NumVertsX - 1; i++)
{
for (int j = 0; j < NumVertsY - 1; j++)
{
indices.Write(j * NumVertsX + i);
indices.Write(j * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write(j * NumVertsX + i);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i);
}
}
indices.Dispose();
}
using (var vertexStream = mesh.GetVertexStream())
{
var vertices = new BinaryWriter(vertexStream);
for (i = 0; i < NumVertsX; i++)
{
for (int j = 0; j < NumVertsY; j++)
{
float wl = .2f;
float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));
vertices.Write((i - NumVertsX * 0.5f) * scale);
vertices.Write(height);
vertices.Write((j - NumVertsY * 0.5f) * scale);
}
}
vertices.Dispose();
}
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.PhyFloat;
* 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.PhyFloat)
* {
* // heightScale isn't applied internally for float data
* height = reader.ReadSingle();
* }
* else if (scalarType == PhyScalarType.PhyUChar)
* {
* 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);
RigidBody ground = LocalCreateRigidBody(0, tr, groundShape);
ground.UserObject = "Ground";
CreateVehicle(vehicleTr);
}
private void CreateVehicle(Matrix transform)
{
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);
VehicleTuning tuning = new VehicleTuning();
IVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(World);
//vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
vehicle = new CustomVehicle(tuning, carChassis, vehicleRayCaster);
carChassis.ActivationState = ActivationState.DisableDeactivation;
World.AddAction(vehicle);
const 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);
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 (int 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 = transform;
}
internal CollisionShape(CollisionShapes _shape)
{
shape = _shape;
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray() { Position = startingTransform.Position, Direction = Toolbox.UpVector };
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit() { Location = startingTransform.Position, Normal = new Vector3(), T = 0 };
return true;
}
}
hit = new RayHit();
BoundingBox boundingBox;
AffineTransform transform = new AffineTransform();
transform.Translation = worldTransform.Position;
Matrix3X3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = Resources.GetTriangle();
var hitElements = Resources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.maximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.maximumRadius < radius)
tri.maximumRadius = radius;
radius = tri.vC.LengthSquared();
if (tri.maximumRadius < radius)
tri.maximumRadius = radius;
tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform();
triangleTransform.Orientation = Quaternion.Identity;
triangleTransform.Position = center;
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.maximumRadius = 0;
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return false;
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present
/// in the entry, this filter will be passed into inner ray casts.</param>
/// <param name="result">Data and hit object from the first impact, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayCastResult result)
{
CompoundChild hitChild;
RayHit rayHit;
bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, filter, out rayHit, out hitChild);
result = new RayCastResult { HitData = rayHit, HitObject = hitChild.CollisionInformation };
return hit;
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
Toolbox.GetExpandedBoundingBox(ref castShape, ref startingTransform, ref sweep, out boundingBox);
var hitElements = Resources.GetCompoundChildList();
if (hierarchy.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.count; i++)
{
var candidate = hitElements.Elements[i].CollisionInformation;
RayHit tempHit;
if (candidate.ConvexCast(castShape, ref startingTransform, ref sweep, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
Resources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
Resources.GiveBack(hitElements);
return false;
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present
/// in the entry, this filter will be passed into inner ray casts.</param>
/// <param name="hit">Hit data, if any.</param>
/// <param name="hitChild">Child hit by the cast.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit, out CompoundChild hitChild)
{
hit = new RayHit();
hitChild = null;
BoundingBox boundingBox;
castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox);
var hitElements = PhysicsResources.GetCompoundChildList();
if (hierarchy.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
var candidate = hitElements.Elements[i].CollisionInformation;
RayHit tempHit;
if (candidate.ConvexCast(castShape, ref startingTransform, ref sweep, filter, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
hitChild = hitElements.Elements[i];
}
}
PhysicsResources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
PhysicsResources.GiveBack(hitElements);
return false;
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create a few basic rigid bodies
BoxShape groundShape = new BoxShape(50, 50, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -51.55f, 0), groundShape);
ground.UserObject = "Ground";
int numLinks = 5;
bool spherical = true;
bool floatingBase = false;
Vector3 basePosition = new Vector3(-0.4f, 3.0f, 0.0f);
Vector3 baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
Vector3 linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
var mb = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase);
floatingBase = !floatingBase;
mb.CanSleep = true;
mb.HasSelfCollision = false;
mb.UseGyroTerm = true;
bool damping = true;
if (damping)
{
mb.LinearDamping = 0.1f;
mb.AngularDamping = 0.9f;
}
else
{
mb.LinearDamping = 0;
mb.AngularDamping = 0;
}
if (numLinks > 0)
{
float q0 = 45.0f * (float)Math.PI / 180.0f;
if (spherical)
{
Quaternion quat0 = Quaternion.RotationAxis(Vector3.Normalize(new Vector3(1, 1, 0)), q0);
quat0.Normalize();
mb.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W });
}
else
{
mb.SetJointPosMultiDof(0, new float[] { q0 });
}
}
AddColliders(mb, baseHalfExtents, linkHalfExtents);
LocalCreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f));
}
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);
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
indexVertexArrays = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts, 3 * sizeof(int), Vector3.SizeInBytes, PhyScalarType.Int32, PhyScalarType.Single);
DataStream indices = mesh.LockIndices();
for (int i = 0; i < NumVertsX - 1; i++)
{
for (int j = 0; j < NumVertsY - 1; j++)
{
indices.Write(j * NumVertsX + i);
indices.Write(j * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write(j * NumVertsX + i);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i);
}
}
indices.Dispose();
indexVertexArrays.AddIndexedMesh(mesh);
convexcastBatch = new ConvexcastBatch(40.0f, 0.0f, -10.0f, 80.0f);
//convexcastBatch = new ConvexcastBatch(true, 40.0f, -50.0f, 50.0f);
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";
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref System.Numerics.Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox localSpaceBoundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out localSpaceBoundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = new QuickList<int>(BufferPools<int>.Thread);
if (Shape.GetOverlaps(localSpaceBoundingBox, ref hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.GetTriangle(hitElements.Elements[i], ref worldTransform, out tri.vA, out tri.vB, out tri.vC);
System.Numerics.Vector3 center;
Vector3Ex.Add(ref tri.vA, ref tri.vB, out center);
Vector3Ex.Add(ref center, ref tri.vC, out center);
Vector3Ex.Multiply(ref center, 1f / 3f, out center);
Vector3Ex.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3Ex.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3Ex.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
tri.MaximumRadius = radius;
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
tri.MaximumRadius = radius;
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform { Orientation = System.Numerics.Quaternion.Identity, Position = center };
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
hitElements.Dispose();
return hit.T != float.MaxValue;
}
PhysicsThreadResources.GiveBack(tri);
hitElements.Dispose();
return false;
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -9.80665f, 0);
const bool floating = false;
const bool gyro = false;
const int numLinks = 1;
const bool canSleep = false;
const bool selfCollide = false;
Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
//Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
Vector3 baseInertiaDiag = Vector3.Zero;
const float baseMass = 0;
multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//multiBody.UseRK4Integration = true;
//multiBody.BaseWorldTransform = Matrix.Identity;
//init the links
Vector3 hingeJointAxis = new Vector3(1, 0, 0);
//y-axis assumed up
Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
for (int i = 0; i < numLinks; i++)
{
const float linkMass = 10;
Vector3 linkInertiaDiag = Vector3.Zero;
using (var shape = new SphereShape(radius))
{
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
}
multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
multiBody.FinalizeMultiDof();
(World as MultiBodyDynamicsWorld).AddMultiBody(multiBody);
multiBody.CanSleep = canSleep;
multiBody.HasSelfCollision = selfCollide;
multiBody.UseGyroTerm = gyro;
#if PENDULUM_DAMPING
multiBody.LinearDamping = 0.1f;
multiBody.AngularDamping = 0.9f;
#else
multiBody.LinearDamping = 0;
multiBody.AngularDamping = 0;
#endif
for (int i = 0; i < numLinks; i++)
{
var shape = new SphereShape(radius);
CollisionShapes.Add(shape);
var col = new MultiBodyLinkCollider(multiBody, i);
col.CollisionShape = shape;
//const bool isDynamic = true;
CollisionFilterGroups collisionFilterGroup = CollisionFilterGroups.Everything; // : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.Everything; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
multiBody.GetLink(i).Collider = col;
}
}
public Physics()
{
ManifoldPoint.ContactAdded += CustomMaterialCombinerCallback;
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
// Set up the vertex data
gVertices = new Vector3[totalVerts];
SetVertexPositions(waveheight, 0.0f);
// Set up the face data
int index = 0;
int[] gIndices = new int[totalTriangles * 3];
for (int y = 0; y < NumVertsY - 1; y++)
{
for (int x = 0; x < NumVertsX - 1; x++)
{
gIndices[index++] = y * NumVertsX + x;
gIndices[index++] = y * NumVertsX + x + 1;
gIndices[index++] = (y + 1) * NumVertsX + x;
gIndices[index++] = y * NumVertsX + x + 1;
gIndices[index++] = (y + 1) * NumVertsX + x + 1;
gIndices[index++] = (y + 1) * NumVertsX + x;
}
}
// Explicitly set up the materials. It's a small array so let's do it bit by bit.
BulletMaterial[] gMaterials = new BulletMaterial[2];
gMaterials[0].Friction = 0;
gMaterials[0].Restitution = 0.9f;
gMaterials[1].Friction = 0.9f;
gMaterials[1].Restitution = 0.1f;
// Set up the face->material index data
int[] gFaceMaterialIndices = new int[totalTriangles];
for (int a = 0; a < totalTriangles; a++)
{
// This will give the first half of the faces low friction and high restitution
// and the second half of the faces high friction and low restitution
if (a > totalTriangles / 2)
{
gFaceMaterialIndices[a] = 0;
}
else
{
gFaceMaterialIndices[a] = 1;
}
}
// Create the array structure
TriangleIndexVertexMaterialArray indexVertexArrays = new TriangleIndexVertexMaterialArray(
gIndices, gVertices, gMaterials, gFaceMaterialIndices);
// Create the multimaterial mesh shape
trimeshShape = new MultimaterialTriangleMeshShape(indexVertexArrays, true);
CollisionShapes.Add(trimeshShape);
// create the ground
//CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShapes.PushBack(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(-15, 0, -30), trimeshShape);
ground.UserObject = "Ground";
ground.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
CollisionShape colShape = new BoxShape(0.5f);
CollisionShapes.Add(colShape);
for (int i = 0; i < 12; i++)
{
RigidBody body = LocalCreateRigidBody(1, Matrix.Translation(10 - i, 10, -20 + i * 3), colShape);
body.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
body.Friction = 0.9f;
body.Gravity = new Vector3(0, -20, 0);
body.ApplyCentralImpulse(new Vector3(-7.7f, 0, 0));
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
CollisionShape groundShape = new BoxShape(20, 50, 10);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0,
Matrix.RotationAxis(new Vector3(0, 0, 1), (float)Math.PI * 0.03f) * Matrix.Translation(0, -50, 0),
groundShape);
ground.Friction = 1;
ground.RollingFriction = 1;
ground.UserObject = "Ground";
groundShape = new BoxShape(100, 50, 100);
CollisionShapes.Add(groundShape);
ground = LocalCreateRigidBody(0, Matrix.Translation(0, -54, 0), groundShape);
ground.Friction = 1;
ground.RollingFriction = 1;
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
CollisionShape[] colShapes =
{
new SphereShape(1),
new CapsuleShape(0.5f, 1),
new CapsuleShapeX(0.5f, 1),
new CapsuleShapeZ(0.5f, 1),
new ConeShape(0.5f, 1),
new ConeShapeX(0.5f, 1),
new ConeShapeZ(0.5f, 1),
new CylinderShape(new Vector3(0.5f, 1, 0.5f)),
new CylinderShapeX(new Vector3(1, 0.5f, 0.5f)),
new CylinderShapeZ(new Vector3(0.5f, 0.5f, 1)),
};
foreach (var collisionShape in colShapes)
{
CollisionShapes.Add(collisionShape);
}
const float mass = 1.0f;
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
var rbInfo = new RigidBodyConstructionInfo(mass, null, null, localInertia);
const float startX = StartPosX - ArraySizeX / 2;
const float startY = StartPosY;
const float startZ = StartPosZ - ArraySizeZ / 2;
int shapeIndex = 0;
for (int k = 0; k < ArraySizeY; k++)
{
for (int i = 0; i < ArraySizeX; i++)
{
for (int j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
2 * i + startX,
2 * k + startY + 20,
2 * j + startZ
);
shapeIndex++;
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
rbInfo.CollisionShape = colShapes[shapeIndex % colShapes.Length];
RigidBody body = new RigidBody(rbInfo);
body.Friction = 1;
body.RollingFriction = 0.3f;
body.SetAnisotropicFriction(colShape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.RollingFriction);
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
}
public override void InitPhysics()
{
int i;
shootBoxInitialSpeed = 4000;
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.BoxShape, BroadphaseNativeType.BoxShape,
CollisionConf.GetCollisionAlgorithmCreateFunc(BroadphaseNativeType.ConvexShape, BroadphaseNativeType.ConvexShape));
Broadphase = new DbvtBroadphase();
// the default constraint solver.
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.SolverInfo.SplitImpulse = 1;
World.SolverInfo.NumIterations = 20;
World.DispatchInfo.UseContinuous = ccdMode;
World.Gravity = new Vector3(0, -10, 0);
BoxShape ground = new BoxShape(200, 1, 200);
// ground.InitializePolyhedralFeatures();
CollisionShapes.Add(ground);
RigidBody body = LocalCreateRigidBody(0, Matrix.Identity, ground);
body.UserObject = "Ground";
CollisionShape shape = new CylinderShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
CollisionShapes.Add(shape);
int numObjects = 120;
for (i = 0; i < numObjects; i++)
{
//stack them
int colsize = 10;
int row = (int)((i * CubeHalfExtents * 2) / (colsize * 2 * CubeHalfExtents));
int row2 = row;
int col = (i) % (colsize) - colsize / 2;
if (col > 3)
{
col = 11;
row2 |= 1;
}
Matrix trans = Matrix.Translation(col * 2 * CubeHalfExtents + (row2 % 2) * CubeHalfExtents,
row * 2 * CubeHalfExtents + CubeHalfExtents + ExtraHeight, 0);
body = LocalCreateRigidBody(1, trans, shape);
if (ccdMode)
{
body.CcdMotionThreshold = CubeHalfExtents;
body.CcdSweptSphereRadius = 0.9f * CubeHalfExtents;
}
}
}
public override void ExitPhysics()
{
if (m_inverseModel != null)
{
Debug.Log("Dispose inverse model " + m_inverseModel.NumBodies);
m_inverseModel.Dispose();
}
Debug.Log("InverseDynamicsExitPhysics");
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
if (m_dynamicsWorld == null)
{
int i;
for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint tc = m_dynamicsWorld.GetConstraint(i);
m_dynamicsWorld.RemoveConstraint(tc);
tc.Dispose();
}
for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--)
{
MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i);
m_dynamicsWorld.RemoveMultiBodyConstraint(mbc);
mbc.Dispose();
}
for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--)
{
MultiBody mb = m_dynamicsWorld.GetMultiBody(i);
m_dynamicsWorld.RemoveMultiBody(mb);
mb.Dispose();
}
for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i];
RigidBody body = RigidBody.Upcast(obj);
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
m_dynamicsWorld.RemoveCollisionObject(obj);
obj.Dispose();
}
}
if (m_multiBody != null)
{
m_multiBody.Dispose();
}
//delete collision shapes
for (int j = 0; j < CollisionShapes.Count; j++)
{
CollisionShape shape = CollisionShapes[j];
shape.Dispose();
}
CollisionShapes.Clear();
m_dynamicsWorld.Dispose();
m_dynamicsWorld = null;
m_solver.Dispose();
m_solver = null;
Broadphase.Dispose();
Broadphase = null;
Dispatcher.Dispose();
Dispatcher = null;
m_pairCache.Dispose();
m_pairCache = null;
CollisionConf.Dispose();
CollisionConf = null;
Debug.Log("After dispose B");
}
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
CollisionShapes.Add(trimeshShape);
// 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
CollisionShapes.Add(trimeshShape2);
//register GIMPACT algorithm
#if BULLET_GIMPACT
GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
#else
//ConcaveConcaveCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
#endif
}
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);
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
indexVertexArrays = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.NumTriangles = totalTriangles;
mesh.NumVertices = totalVerts;
mesh.TriangleIndexStride = 3 * sizeof(int);
mesh.VertexStride = Vector3.SizeInBytes;
mesh.TriangleIndexBase = Marshal.AllocHGlobal(mesh.TriangleIndexStride * totalTriangles);
mesh.VertexBase = Marshal.AllocHGlobal(mesh.VertexStride * totalVerts);
var indicesStream = mesh.GetTriangleStream();
var indices = new BinaryWriter(indicesStream);
for (int i = 0; i < NumVertsX - 1; i++)
{
for (int j = 0; j < NumVertsY - 1; j++)
{
indices.Write(j * NumVertsX + i);
indices.Write(j * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write(j * NumVertsX + i);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i);
}
}
indices.Dispose();
indexVertexArrays.AddIndexedMesh(mesh);
raycastBar = new RaycastBar(4000.0f, 0.0f);
//raycastBar = new RaycastBar(true, 40.0f, -50.0f, 50.0f);
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
for (int i = 0; i < 10; i++)
{
//CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
Matrix startTransform = Matrix.Translation(2 * i, 10, 1);
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";
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
const float THETA = (float)Math.PI / 4.0f;
float L_1 = 2 - (float)Math.Tan(THETA);
float L_2 = 1 / (float)Math.Cos(THETA);
float RATIO = L_2 / L_1;
RigidBody bodyA;
RigidBody bodyB;
CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
CompoundShape cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
float mass = 6.28f;
Vector3 localInertia;
cyl0.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);
body = new RigidBody(ci); //1,0,cyl0,localInertia);
World.AddRigidBody(body);
body.LinearFactor = Vector3.Zero;
body.AngularFactor = new Vector3(0, 1, 0);
bodyA = body;
cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
cylB = new CylinderShape(L_2, 0.025f, L_2);
cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
mass = 6.28f;
cyl0.CalculateLocalInertia(mass, out localInertia);
ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);
body = new RigidBody(ci);//1,0,cyl0,localInertia);
body.LinearFactor = Vector3.Zero;
HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);
World.AddConstraint(hinge);
bodyB = body;
body.AngularVelocity = new Vector3(0, 3, 0);
World.AddRigidBody(body);
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
Matrix mat = Matrix.RotationQuaternion(orn);
axisB = new Vector3(mat.M21, mat.M22, mat.M23);
GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);
World.AddConstraint(gear, true);
mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
#if P2P
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
#else
{
hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
const float targetVelocity = 1.0f;
const float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
#endif
RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbA1.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbB1.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
//spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
spSlider1.LowerLinearLimit = -15.0f;
spSlider1.UpperLinearLimit = -5.0f;
spSlider1.LowerLinearLimit = 5.0f;
spSlider1.UpperLinearLimit = 15.0f;
spSlider1.LowerLinearLimit = -10.0f;
spSlider1.UpperLinearLimit = -10.0f;
spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;
World.AddConstraint(spSlider1, true);
spSlider1.DebugDrawSize = 5.0f;
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
const float angle = 0; //SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
{
LinearLowerLimit = lowerSliderLimit,
LinearUpperLimit = hiSliderLimit,
//range should be small, otherwise singularities will 'explode' the constraint
//AngularLowerLimit = new Vector3(-1.5f,0,0),
//AngularUpperLimit = new Vector3(1.5f,0,0),
//AngularLowerLimit = new Vector3(0,0,0),
//AngularUpperLimit = new Vector3(0,0,0),
AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
AngularUpperLimit = new Vector3(1.5f, 0, 0)
};
//spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
{
LinearUpperLimit = new Vector3(5, 0, 0),
LinearLowerLimit = new Vector3(-5, 0, 0),
AngularLowerLimit = new Vector3(0, 0, -1.5f),
AngularUpperLimit = new Vector3(0, 0, 1.5f),
DebugDrawSize = 5
};
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
axisA = new Vector3(0, 1, 0);
axisB = new Vector3(0, 1, 0);
Vector3 pivotA2 = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create a few basic rigid bodies
BoxShape groundShape = new BoxShape(50, 50, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
const float mass = 1.0f;
BoxShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
const float start_x = StartPosX - ArraySizeX / 2;
const float start_y = StartPosY;
const float start_z = StartPosZ - ArraySizeZ / 2;
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
3 * i + start_x,
3 * k + start_y,
3 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
World.AddRigidBody(body);
}
}
}
var settings = new MultiBodySettings()
{
BasePosition = new Vector3(60, 29.5f, -2) * Scaling,
CanSleep = true,
CreateConstraints = true,
DisableParentCollision = true, // the self-collision has conflicting/non-resolvable contact normals
IsFixedBase = false,
NumLinks = 2,
UsePrismatic = true
};
var multiBodyA = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.NumLinks = 10;
settings.BasePosition = new Vector3(0, 29.5f, -settings.NumLinks * 4);
settings.IsFixedBase = true;
settings.UsePrismatic = false;
var multiBodyB = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.BasePosition = new Vector3(-20 * Scaling, 29.5f * Scaling, -settings.NumLinks * 4 * Scaling);
settings.IsFixedBase = false;
var multiBodyC = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.BasePosition = new Vector3(-20, 9.5f, -settings.NumLinks * 4);
settings.IsFixedBase = true;
settings.UsePrismatic = true;
settings.DisableParentCollision = true;
var multiBodyPrim = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
CreateGround();
//CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
var wo = WavefrontObj.Load("data/file.obj");
if (wo.Indices.Count == 0)
{
return;
}
var localScaling = new Vector3(6, 6, 6);
triangleMesh = CreateTriangleMesh(wo.Indices, wo.Vertices, localScaling);
// Convex hull approximation
ConvexHullShape convexShape = CreateHullApproximation(triangleMesh);
CollisionShapes.Add(convexShape);
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
// Non-moving body
Vector3 convexDecompositionObjectOffset = new Vector3(10, 0, 0);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(triangleMesh, useQuantization);
CollisionShapes.Add(concaveShape);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
var hacd = new Hacd()
{
VerticesPerConvexHull = 100,
CompacityWeight = 0.1,
VolumeWeight = 0,
// Recommended HACD parameters
NClusters = 2,
Concavity = 100,
AddExtraDistPoints = false,
AddFacesPoints = false,
AddNeighboursDistPoints = false
};
hacd.SetPoints(wo.Vertices);
hacd.SetTriangles(wo.Indices);
hacd.Compute();
hacd.Save("output.wrl", false);
// Generate convex result
var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
var writer = new StreamWriter(outputFile);
var convexDecomposition = new ConvexDecomposition(writer)
{
LocalScaling = localScaling
};
for (int c = 0; c < hacd.NClusters; c++)
{
int nVertices = hacd.GetNPointsCH(c);
int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
double[] points = new double[nVertices * 3];
long[] triangles = new long[trianglesLen];
hacd.GetCH(c, points, triangles);
if (trianglesLen == 0)
{
continue;
}
Vector3[] verticesArray = new Vector3[nVertices];
int vi3 = 0;
for (int vi = 0; vi < nVertices; vi++)
{
verticesArray[vi] = new Vector3(
(float)points[vi3], (float)points[vi3 + 1], (float)points[vi3 + 2]);
vi3 += 3;
}
int[] trianglesInt = new int[trianglesLen];
for (int ti = 0; ti < trianglesLen; ti++)
{
trianglesInt[ti] = (int)triangles[ti];
}
convexDecomposition.Result(verticesArray, trianglesInt);
}
writer.Dispose();
outputFile.Dispose();
// Combine convex shapes into a compound shape
var compound = new CompoundShape();
for (int i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
var convexShape2 = convexDecomposition.convexShapes[i];
Matrix trans = Matrix.Translation(centroid);
if (enableSat)
{
convexShape2.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape2);
compound.AddChildShape(trans, convexShape2);
LocalCreateRigidBody(1.0f, trans, convexShape2);
}
CollisionShapes.Add(compound);
#if true
mass = 10.0f;
var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
convexDecompositionObjectOffset = new Vector3(10, 0, 0);
// Load wavefront file
var wo = new WavefrontObj();
int tcount = wo.LoadObj("data/file.obj");
if (tcount == 0)
{
return;
}
// Convert file data to TriangleMesh
var trimesh = new TriangleMesh();
trimeshes.Add(trimesh);
Vector3 localScaling = new Vector3(6, 6, 6);
List <int> indices = wo.Indices;
List <Vector3> vertices = wo.Vertices;
int i;
for (i = 0; i < tcount; i++)
{
int index0 = indices[i * 3];
int index1 = indices[i * 3 + 1];
int index2 = indices[i * 3 + 2];
Vector3 vertex0 = vertices[index0] * localScaling;
Vector3 vertex1 = vertices[index1] * localScaling;
Vector3 vertex2 = vertices[index2] * localScaling;
trimesh.AddTriangleRef(ref vertex0, ref vertex1, ref vertex2);
}
// Create a hull approximation
ConvexHullShape convexShape;
using (var tmpConvexShape = new ConvexTriangleMeshShape(trimesh))
{
using (var hull = new ShapeHull(tmpConvexShape))
{
hull.BuildHull(tmpConvexShape.Margin);
convexShape = new ConvexHullShape(hull.Vertices);
}
}
if (sEnableSAT)
{
convexShape.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape);
// Add non-moving body to world
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
CollisionShapes.Add(concaveShape);
// HACD
var hacd = new Hacd();
hacd.SetPoints(wo.Vertices);
hacd.SetTriangles(wo.Indices);
hacd.CompacityWeight = 0.1;
hacd.VolumeWeight = 0.0;
// Recommended HACD parameters: 2 100 false false false
hacd.NClusters = 2; // minimum number of clusters
hacd.Concavity = 100; // maximum concavity
hacd.AddExtraDistPoints = false;
hacd.AddNeighboursDistPoints = false;
hacd.AddFacesPoints = false;
hacd.NumVerticesPerConvexHull = 100; // max of 100 vertices per convex-hull
hacd.Compute();
hacd.Save("output.wrl", false);
// Generate convex result
var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
var writer = new StreamWriter(outputFile);
var convexDecomposition = new ConvexDecomposition(writer, this);
convexDecomposition.LocalScaling = localScaling;
for (int c = 0; c < hacd.NClusters; c++)
{
Vector3[] points;
int[] triangles;
hacd.GetCH(c, out points, out triangles);
convexDecomposition.ConvexDecompResult(points, triangles);
}
// Combine convex shapes into a compound shape
var compound = new CompoundShape();
for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
Matrix trans = Matrix.Translation(centroid);
var convexShape2 = convexDecomposition.convexShapes[i] as ConvexHullShape;
if (sEnableSAT)
{
convexShape2.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape2);
compound.AddChildShape(trans, convexShape2);
LocalCreateRigidBody(1.0f, trans, convexShape2);
}
CollisionShapes.Add(compound);
writer.Dispose();
outputFile.Dispose();
#if true
mass = 10.0f;
var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
string bulletFile;
string[] args = Environment.GetCommandLineArgs();
if (args.Length == 1)
{
bulletFile = "testFile.bullet";
}
else
{
bulletFile = args[1];
}
BulletWorldImporter fileLoader = new CustomBulletWorldImporter(World);
if (!fileLoader.LoadFile(bulletFile))
{
CollisionShape groundShape = new BoxShape(50);
CollisionShapes.Add(groundShape);
RigidBody ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
float mass = 1.0f;
Vector3[] positions = new Vector3[2] {
new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f)
};
float[] radi = new float[2] {
0.3f, 0.4f
};
CollisionShape colShape = new MultiSphereShape(positions, radi);
//CollisionShape colShape = new CapsuleShapeZ(1, 1);
//CollisionShape colShape = new CylinderShapeZ(1, 1, 1);
//CollisionShape colShape = new BoxShape(1);
//CollisionShape colShape = new SphereShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
float start_x = StartPosX - ArraySizeX / 2;
float start_y = StartPosY;
float start_z = StartPosZ - ArraySizeZ / 2;
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
2 * i + start_x,
2 * k + start_y,
2 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
DefaultSerializer serializer = new DefaultSerializer();
serializer.RegisterNameForObject(ground, "GroundName");
for (i = 0; i < CollisionShapes.Count; i++)
{
serializer.RegisterNameForObject(CollisionShapes[i], "name" + i.ToString());
}
Point2PointConstraint p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0));
World.AddConstraint(p2p);
serializer.RegisterNameForObject(p2p, "constraintje");
World.Serialize(serializer);
BulletSharp.DataStream data = serializer.LockBuffer();
byte[] dataBytes = new byte[data.Length];
data.Read(dataBytes, 0, dataBytes.Length);
FileStream file = new FileStream("testFile.bullet", FileMode.Create);
file.Write(dataBytes, 0, dataBytes.Length);
file.Close();
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
BoxShape groundShape = new BoxShape(50, 1, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
const float mass = 1.0f;
BoxShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
const float startX = StartPosX - ArraySizeX / 2;
const float startY = StartPosY;
const float startZ = StartPosZ - ArraySizeZ / 2;
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
2 * i + startX,
2 * k + startY,
2 * j + startZ
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
RigidBody body = new RigidBody(rbInfo);
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox);
var tri = PhysicsResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
tri.MaximumRadius = radius;
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
tri.MaximumRadius = radius;
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform { Orientation = Quaternion.Identity, Position = center };
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return false;
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="rayHit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref System.Numerics.Vector3 sweep, out RayHit rayHit)
{
CompoundChild hitChild;
bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, out rayHit, out hitChild);
return hit;
}
