/// <summary> /// Initializes a new instance of the <see cref="CapsuleColliderShape"/> class. /// </summary> /// <param name="is2D">if set to <c>true</c> [is2 d].</param> /// <param name="radius">The radius.</param> /// <param name="length">The length of the capsule.</param> /// <param name="orientation">Up axis.</param> public CapsuleColliderShape(bool is2D, float radius, float length, ShapeOrientation orientation) { Type = ColliderShapeTypes.Capsule; Is2D = is2D; capsuleLength = length; capsuleRadius = radius; Matrix rotation; CapsuleShape shape; switch (orientation) { case ShapeOrientation.UpX: shape = new CapsuleShapeZ(radius, length); rotation = Matrix.RotationX((float)Math.PI / 2.0f); break; case ShapeOrientation.UpY: shape = new CapsuleShape(radius, length); rotation = Matrix.Identity; break; case ShapeOrientation.UpZ: shape = new CapsuleShapeX(radius, length); rotation = Matrix.RotationZ((float)Math.PI / 2.0f); break; default: throw new ArgumentOutOfRangeException("orientation"); } InternalShape = Is2D ? (CollisionShape)new Convex2DShape(shape) { LocalScaling = new Vector3(1, 1, 0) }: shape; DebugPrimitiveMatrix = Matrix.Scaling(new Vector3(1.01f)) * rotation; }
public DynamicCharacterController(CollisionWorld collisionWorld, RigidBody body, CapsuleShape shape, short staticRaycastGroup, short staticRaycastMask) { _gravity = body.Gravity; _findGroundAndSteps = new FindGroundAndSteps(this, collisionWorld, staticRaycastGroup, staticRaycastMask); RigidBody = body; CapsuleRadius = shape.Radius; CapsuleHalfHeight = shape.HalfHeight; RigidBody.Friction = 0; RigidBody.RollingFriction = 0; SetupBody(); ResetStatus(); }
public static TriggerRegion CreateCapsuleTriggerRegion( string name, TriggerReportEvent trh, float radius, float height, Vector3 position, Quaternion orientation) { var csm = LKernel.GetG<CollisionShapeManager>(); CollisionShape shape; if (!csm.TryGetShape(name, out shape)) { shape = new CapsuleShape(radius, height); csm.RegisterShape(name, shape); } var tr = new TriggerRegion(name, position, orientation, shape); tr.OnTrigger += trh; AddToDispose(tr, trh); return tr; }
Mesh CreateCapsuleShape(CapsuleShape shape) { int upAxis = shape.UpAxis; Vector3 size = shape.ImplicitShapeDimensions; float halfHeight = size[upAxis]; float radius = (upAxis == 0) ? size.Y : size.X; // Combine a cylinder and two spheres. Mesh cylinder = Mesh.CreateCylinder(device, radius, radius, halfHeight * 2, 8, 1); Mesh sphere = Mesh.CreateSphere(device, radius, 8, 4); Matrix[] transforms; if (upAxis == 0) { transforms = new Matrix[] { Matrix.Translation(halfHeight, 0, 0), Matrix.RotationY((float)Math.PI / 2), Matrix.Translation(-halfHeight, 0, 0)}; } else if (upAxis == 1) { transforms = new Matrix[] { Matrix.Translation(0, halfHeight, 0), Matrix.RotationX((float)Math.PI / 2), Matrix.Translation(0, -halfHeight, 0)}; } else { transforms = new Matrix[] { Matrix.Translation(0, 0, halfHeight), Matrix.Identity, Matrix.Translation(0, 0, -halfHeight)}; } Mesh[] meshes = new Mesh[] { sphere, cylinder, sphere }; Mesh mesh = Mesh.Concatenate(device, meshes, MeshFlags.Managed, transforms, null); cylinder.Dispose(); sphere.Dispose(); complexShapes.Add(shape, mesh); return mesh; }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000)); Solver = new SequentialImpulseConstraintSolver(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf); World.DispatchInfo.AllowedCcdPenetration = 0.0001f; //World.Gravity = Freelook.Up * -10.0f; Matrix startTransform = Matrix.Translation(10.210098f, -1.6433364f, 16.453260f); ghostObject = new PairCachingGhostObject(); ghostObject.WorldTransform = startTransform; Broadphase.OverlappingPairCache.SetInternalGhostPairCallback(new GhostPairCallback()); const float characterHeight = 1.75f; const float characterWidth = 1.75f; ConvexShape capsule = new CapsuleShape(characterWidth, characterHeight); ghostObject.CollisionShape = capsule; ghostObject.CollisionFlags = CollisionFlags.CharacterObject; const float stepHeight = 0.35f; character = new KinematicCharacterController(ghostObject, capsule, stepHeight); BspLoader bspLoader = new BspLoader(); bspLoader.LoadBspFile("data/BspDemo.bsp"); BspConverter bsp2Bullet = new BspToBulletConverter(this); bsp2Bullet.ConvertBsp(bspLoader, 0.1f); World.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter); World.AddAction(character); convexResultCallback = new ClosestConvexResultCallback(); convexResultCallback.CollisionFilterMask = (short)CollisionFilterGroups.StaticFilter; cameraSphere = new SphereShape(0.2f); }
/// <inheritdoc/> public override BulletSharp.RigidBody CreateRigidBody(float mass) { var ppm = DD.Physics.PhysicsSimulator.PPM; var mstate = new DefaultMotionState (); var shape = new BulletSharp.CapsuleShape (radius/ppm, halfHeight/ppm); var info = new BulletSharp.RigidBodyConstructionInfo (mass, mstate, shape); return new BulletSharp.RigidBody (info); }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup DefaultCollisionConstructionInfo cci = new DefaultCollisionConstructionInfo(); cci.DefaultMaxPersistentManifoldPoolSize = 32768; CollisionConf = new DefaultCollisionConfiguration(cci); 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(); Solver = new SequentialImpulseConstraintSolver(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf); World.Gravity = new Vector3(0, -10, 0); 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 TestRig(DynamicsWorld ownerWorld, Vector3 positionOffset, bool bFixed) { this.ownerWorld = ownerWorld; Vector3 vUp = new Vector3(0, 1, 0); // // Setup geometry // const float fBodySize = 0.25f; const float fLegLength = 0.45f; const float fForeLegLength = 0.75f; const float PI_2 = (float)(0.5f * Math.PI); const float PI_4 = (float)(0.25f * Math.PI); const float PI_8 = (float)(0.125f * Math.PI); shapes[0] = new CapsuleShape(fBodySize, 0.10f); int i; for (i = 0; i < NumLegs; i++) { shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength); shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength); } // // Setup rigid bodies // const float fHeight = 0.5f; Matrix offset = Matrix.Translation(positionOffset); // root Vector3 vRoot = new Vector3(0, fHeight, 0); Matrix transform = Matrix.Translation(vRoot); if (bFixed) { bodies[0] = LocalCreateRigidBody(0, transform * offset, shapes[0]); } else { bodies[0] = LocalCreateRigidBody(1, transform * offset, shapes[0]); } // legs for (i = 0; i < NumLegs; i++) { float fAngle = (float)(2 * Math.PI * i / NumLegs); float fSin = (float)Math.Sin(fAngle); float fCos = (float)Math.Cos(fAngle); Vector3 vBoneOrigin = new Vector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength)); // thigh Vector3 vToBone = (vBoneOrigin - vRoot); vToBone.Normalize(); Vector3 vAxis = Vector3.Cross(vToBone, vUp); transform = Matrix.RotationQuaternion(Quaternion.RotationAxis(vAxis, PI_2)) * Matrix.Translation(vBoneOrigin); bodies[1 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[1 + 2 * i]); // shin transform = Matrix.Translation(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength)); bodies[2 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[2 + 2 * i]); } // Setup some damping on the bodies for (i = 0; i < BodyPartCount; ++i) { bodies[i].SetDamping(0.05f, 0.85f); bodies[i].DeactivationTime = 0.8f; //bodies[i].SetSleepingThresholds(1.6f, 2.5f); bodies[i].SetSleepingThresholds(0.5f, 0.5f); } // // Setup the constraints // HingeConstraint hingeC; //ConeTwistConstraint coneC; Matrix localA, localB, localC; for (i = 0; i < NumLegs; i++) { float fAngle = (float)(2 * Math.PI * i / NumLegs); float fSin = (float)Math.Sin(fAngle); float fCos = (float)Math.Cos(fAngle); // hip joints localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * fBodySize, 0, fSin * fBodySize); // OK localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform); hingeC = new HingeConstraint(bodies[0], bodies[1 + 2 * i], localA, localB); hingeC.SetLimit(-0.75f * PI_4, PI_8); //hingeC.SetLimit(-0.1f, 0.1f); joints[2 * i] = hingeC; ownerWorld.AddConstraint(joints[2 * i], true); // knee joints localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * (fBodySize + fLegLength), 0, fSin * (fBodySize + fLegLength)); localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform); localC = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[2 + 2 * i].WorldTransform); hingeC = new HingeConstraint(bodies[1 + 2 * i], bodies[2 + 2 * i], localB, localC); //hingeC.SetLimit(-0.01f, 0.01f); hingeC.SetLimit(-PI_8, 0.2f); joints[1 + 2 * i] = hingeC; ownerWorld.AddConstraint(joints[1 + 2 * i], true); } }
public ShapeData CreateCapsule(CapsuleShape shape) { int up = shape.UpAxis; float radius = shape.Radius; float cylinderHalfHeight = shape.HalfHeight; int slices = (int)(radius * 10.0f); int stacks = (int)(radius * 10.0f); slices = (slices > 16) ? 16 : (slices < 3) ? 3 : slices; stacks = (stacks > 16) ? 16 : (stacks < 3) ? 3 : stacks; float hAngleStep = (float)Math.PI * 2 / slices; float vAngleStep = (float)Math.PI / stacks; ShapeData shapeData = new ShapeData(); shapeData.VertexCount = 2 + slices * (stacks - 1); shapeData.IndexCount = 6 * slices * (stacks - 1); Vector3[] vertices = new Vector3[shapeData.VertexCount * 2]; ushort[] indices = new ushort[shapeData.IndexCount]; int i = 0, v = 0; // Vertices // Top and bottom vertices[v++] = GetVectorByAxis(0, -cylinderHalfHeight - radius, 0, up); vertices[v++] = GetVectorByAxis(-Vector3.UnitY, up); vertices[v++] = GetVectorByAxis(0, cylinderHalfHeight + radius, 0, up); vertices[v++] = GetVectorByAxis(Vector3.UnitY, up); // Stacks int j, k; float angle = 0; float vAngle = -(float)Math.PI / 2; Vector3 vTemp; Vector3 cylinderOffset = GetVectorByAxis(0, -cylinderHalfHeight, 0, up); for (j = 0; j < stacks - 1; j++) { float prevAngle = vAngle; vAngle += vAngleStep; if (vAngle > 0 && prevAngle < 0) { cylinderOffset = GetVectorByAxis(0, cylinderHalfHeight, 0, up); } for (k = 0; k < slices; k++) { angle += hAngleStep; vTemp = GetVectorByAxis((float)Math.Cos(vAngle) * (float)Math.Sin(angle), (float)Math.Sin(vAngle), (float)Math.Cos(vAngle) * (float)Math.Cos(angle), up); vertices[v++] = vTemp * radius + cylinderOffset; vertices[v++] = Vector3.Normalize(vTemp); } } // Indices // Top cap ushort index = 2; for (k = 0; k < slices; k++) { indices[i++] = index++; indices[i++] = 0; indices[i++] = index; } indices[i - 1] = 2; // Stacks int sliceDiff = slices * 3; for (j = 0; j < stacks - 2; j++) { for (k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 2]; indices[i + 1] = index++; indices[i + 2] = indices[i - sliceDiff]; i += 3; } for (k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 1]; indices[i + 1] = indices[i - sliceDiff]; indices[i + 2] = indices[i - sliceDiff + 4]; i += 3; } indices[i - 1] = indices[i - sliceDiff]; } // Bottom cap index--; for (k = 0; k < slices; k++) { indices[i++] = index--; indices[i++] = 1; indices[i++] = index; } indices[i - 1] = indices[i - sliceDiff]; shapeData.SetVertexBuffer(device, vertices); shapeData.SetIndexBuffer(device, indices); return shapeData; }
public override CollisionShape GetCollisionShape() { if (collisionShapePtr == null) { CapsuleShape cs = null; if (upAxis == CapsuleAxis.x) { cs = new CapsuleShapeX(radius, height); } else if (upAxis == CapsuleAxis.y) { cs = new CapsuleShape(radius, height); } else if (upAxis == CapsuleAxis.z) { cs = new CapsuleShapeZ(radius, height); } else { Debug.LogError("invalid axis value"); } cs.LocalScaling = m_localScaling.ToBullet(); collisionShapePtr = cs; } return collisionShapePtr; }
public static UnityEngine.Vector3[] CreateCapsule(CapsuleShape shape, out int[] indices) { int up = shape.UpAxis; float radius = shape.Radius; float cylinderHalfHeight = shape.HalfHeight; int slices = (int)(radius * 10.0f); int stacks = (int)(radius * 10.0f); slices = (slices > 16) ? 16 : (slices < 3) ? 3 : slices; stacks = (stacks > 16) ? 16 : (stacks < 3) ? 3 : stacks; float hAngleStep = (float)Math.PI * 2 / slices; float vAngleStep = (float)Math.PI / stacks; int vertexCount = 2 + slices * (stacks - 1); int indexCount = 6 * slices * (stacks - 1); UnityEngine.Vector3[] vertices = new UnityEngine.Vector3[vertexCount * 2]; indices = new int[indexCount]; int i = 0, v = 0; // Vertices // Top and bottom vertices[v++] = GetVectorByAxis(0, -cylinderHalfHeight - radius, 0, up); vertices[v++] = GetVectorByAxis(-UnityEngine.Vector3.up, up); vertices[v++] = GetVectorByAxis(0, cylinderHalfHeight + radius, 0, up); vertices[v++] = GetVectorByAxis(UnityEngine.Vector3.up, up); // Stacks int j, k; float angle = 0; float vAngle = -(float)Math.PI / 2; UnityEngine.Vector3 vTemp; UnityEngine.Vector3 cylinderOffset = GetVectorByAxis(0, -cylinderHalfHeight, 0, up); for (j = 0; j < stacks - 1; j++) { float prevAngle = vAngle; vAngle += vAngleStep; if (vAngle > 0 && prevAngle < 0) { cylinderOffset = GetVectorByAxis(0, cylinderHalfHeight, 0, up); } for (k = 0; k < slices; k++) { angle += hAngleStep; vTemp = GetVectorByAxis((float)Math.Cos(vAngle) * (float)Math.Sin(angle), (float)Math.Sin(vAngle), (float)Math.Cos(vAngle) * (float)Math.Cos(angle), up); vertices[v++] = vTemp * radius + cylinderOffset; vertices[v++] = UnityEngine.Vector3.Normalize(vTemp); } } // Indices // Top cap int index = 2; for (k = 0; k < slices; k++) { indices[i++] = index++; indices[i++] = 0; indices[i++] = index; } indices[i - 1] = 2; // Stacks int sliceDiff = slices * 3; for (j = 0; j < stacks - 2; j++) { for (k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 2]; indices[i + 1] = index++; indices[i + 2] = indices[i - sliceDiff]; i += 3; } for (k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 1]; indices[i + 1] = indices[i - sliceDiff]; indices[i + 2] = indices[i - sliceDiff + 4]; i += 3; } indices[i - 1] = indices[i - sliceDiff]; } // Bottom cap index--; for (k = 0; k < slices; k++) { indices[i++] = index--; indices[i++] = 1; indices[i++] = index; } indices[i - 1] = indices[i - sliceDiff]; return vertices; }
public CollisionShape CreateCapsuleShapeY(float radius, float height) { CapsuleShape shape = new CapsuleShape(radius, height); _allocatedCollisionShapes.Add(shape); return shape; }