/// <summary>
/// Se crea una capsula a partir de un radio, una altura y una posicion.
/// Los valores de la masa y el calculo de inercia asociado estan fijos para que no haya comportamiento erratico.
/// </summary>
/// <param name="radius"></param>
/// <param name="height"></param>
/// <param name="position"></param>
/// <returns></returns>
public static RigidBody CreateCapsule(float radius, float height, TGCVector3 position)
{
//Creamos el shape de la Capsula a partir de un radio y una altura.
var caspsuleShape = new CapsuleShape(radius, height);
//Armamos las transformaciones que luego formaran parte del cuerpo rigido de la capsula.
var capsuleTransform = TGCMatrix.Identity;
capsuleTransform.Origin = position;
var capsuleMotionState = new DefaultMotionState(Matrix.Translation(position.ToBsVector));
// Utilizamos una masa muy grande (1000 Kg) para calcular el momento de inercia de forma que la capsula no
// genere una rotacion y termine volcando.
var capsuleInertia = caspsuleShape.CalculateLocalInertia(100000);
// Aqui usamos una masa bastante baja (1 Kg) para que cuando se arme el cuerpo rigido y se intente aplicar
// un impulso se facil de mover la capsula.
var capsuleRigidBodyInfo = new RigidBodyConstructionInfo(1, capsuleMotionState, caspsuleShape, capsuleInertia);
var localCapsuleRigidBody = new RigidBody(capsuleRigidBodyInfo);
localCapsuleRigidBody.LinearFactor = TGCVector3.One.ToBsVector;
localCapsuleRigidBody.SetDamping(0.5f, 0f);
localCapsuleRigidBody.Restitution = 0f;
localCapsuleRigidBody.Friction = 1;
return(localCapsuleRigidBody);
}
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);
}
private void Draw(CapsuleShape shape, RigidBody body, Color color)
{
const int Segments = 16;
const int Rings = 5;
float l = shape.Length / 2;
float r = shape.Radius;
quat orientation = body.Orientation.ToQuat();
vec3 p = body.Position.ToVec3();
vec3 v = vec3.UnitY * orientation;
for (int i = 0; i < Rings; i++)
{
float radius = (float)Math.Cos(Constants.PiOverTwo / Rings * i) * r;
float offset = r / Rings * i;
primitives.DrawCircle(radius, p + v * (l + offset), orientation, color, Segments);
primitives.DrawCircle(radius, p - v * (l + offset), orientation, color, Segments);
}
for (int i = 0; i < Segments; i++)
{
vec2 d = Utilities.Direction(Constants.TwoPi / Segments * i) * r;
vec3 point = orientation * new vec3(d.x, 0, d.y) + p;
primitives.DrawLine(point + v * l, point - v * l, color);
}
}
/// <summary>
/// Se crea una capsula a partir de un radio, altura, posicion, masa y si se dedea o no calcular
/// la inercia. Esto es importante ya que sin inercia no se generan rotaciones que no se
/// controlen en forma particular.
/// </summary>
/// <param name="radius">Radio de la Capsula</param>
/// <param name="height">Altura de la Capsula</param>
/// <param name="position">Posicion de la Capsula</param>
/// <param name="mass">Masa de la Capsula</param>
/// <param name="needInertia">Booleano para el momento de inercia de la Capsula</param>
/// <returns>Rigid Body de una Capsula</returns>
public static RigidBody CreateCapsule(float radius, float height, TGCVector3 position, float mass, bool needInertia)
{
//Creamos el shape de la Capsula a partir de un radio y una altura.
var capsuleShape = new CapsuleShape(radius, height);
//Armamos las transformaciones que luego formaran parte del cuerpo rigido de la capsula.
var capsuleTransform = TGCMatrix.Identity;
capsuleTransform.Origin = position;
var capsuleMotionState = new DefaultMotionState(capsuleTransform.ToBsMatrix);
RigidBodyConstructionInfo capsuleRigidBodyInfo;
//Calculamos o no el momento de inercia dependiendo de que comportamiento
//queremos que tenga la capsula.
if (!needInertia)
{
capsuleRigidBodyInfo = new RigidBodyConstructionInfo(mass, capsuleMotionState, capsuleShape);
}
else
{
var capsuleInertia = capsuleShape.CalculateLocalInertia(mass);
capsuleRigidBodyInfo = new RigidBodyConstructionInfo(mass, capsuleMotionState, capsuleShape, capsuleInertia);
}
var localCapsuleRigidBody = new RigidBody(capsuleRigidBodyInfo);
localCapsuleRigidBody.LinearFactor = TGCVector3.One.ToBsVector;
//Dado que hay muchos parametros a configurar el RigidBody lo ideal es que
//cada caso se configure segun lo que se necesite.
return(localCapsuleRigidBody);
}
/// <summary>
/// Adds the child shape.
/// </summary>
/// <param name="localTransform">The local transform.</param>
/// <param name="shape">The shape.</param>
public void AddChildShape(float4x4 localTransform, ICapsuleShapeImp shape)
{
var btChildShape = new CapsuleShape(shape.Radius, shape.HalfHeight);
var btLocalTransform = Translator.Float4X4ToBtMatrix(localTransform);
BtCompoundShape.AddChildShape(btLocalTransform, btChildShape);
}
/// <summary>
/// To create a rigid
/// </summary>
/// <param name="rigidBodyData">Rigid data</param>
private void CreateRigid(List <RigidBodyData> rigidBodyData)
{
foreach (var r in rigidBodyData)
{
var tempRigidBodyData = new TempRigidBodyData(r);
//Debug.WriteLine("名字: {0}, 质量: {1}， 排斥力: {2}， 摩擦: {3}， 移动衰减: {4}， 旋转衰减: {5}",
// r.RigidBodyName, r.Mass, r.Repulsion, r.Friction, r.MoveAttenuation, r.Rotation);
var init_matrix = tempRigidBodyData.init_matrix;
this.tempRigidBodyData.Add(tempRigidBodyData);
RigidBody rigidBody = null;
CollisionShape collisionShape;
switch (r.Shape)
{
case RigidBodyShape.Sphere: // 球体
collisionShape = new SphereShape(r.Size.X);
break;
case RigidBodyShape.Box: // box
collisionShape = new BoxShape(r.Size.X, r.Size.Y, r.Size.Z);
break;
case RigidBodyShape.Capsule: // capsule
collisionShape = new CapsuleShape(r.Size.X, r.Size.Y);
break;
default: // Exception handling
throw new System.Exception("Invalid rigid body data");
}
//质量， 排斥力， 摩擦， 移动衰减， 旋转衰减
var rigidProperty = new RigidProperty(r.Mass, r.Repulsion, r.Friction, r.MoveAttenuation, r.RotationAttenuation);
var superProperty = new SuperProperty(r.PhysicsCalcType == PhysicsCalcType.Static, (CollisionFilterGroups)(1 << r.RigidBodyGroup), (CollisionFilterGroups)r.UnCollisionGroupFlag);
rigidBody = this.bulletManager.CreateRigidBody(collisionShape, init_matrix, rigidProperty, superProperty);
this.rigidBodies.Add(rigidBody);
}
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public GeneralConvexPairStressDemo(DemosGame game)
: base(game)
{
Space.Remove(vehicle.Vehicle);
//Enable simplex caching.
ConfigurationHelper.ApplySuperSpeedySettings(Space);
for (int i = 0; i < 2000; i++)
{
EntityShape shape;
switch (i % 3)
{
case 0:
shape = new CylinderShape(0.5m + (Fix64)random.NextDouble() * 1.5m, 0.5m + (Fix64)random.NextDouble() * 1.5m);
break;
case 1:
shape = new ConeShape(0.5m + (Fix64)random.NextDouble() * 1.5m, 0.5m + (Fix64)random.NextDouble() * 1.5m);
break;
default:
shape = new CapsuleShape(0.5m + (Fix64)random.NextDouble() * 1.5m, 0.5m + (Fix64)random.NextDouble() * 1.5m);
break;
}
var toAdd = new Entity(shape, 2);
//toAdd.LocalInertiaTensorInverse = new BEPUutilities.Matrix3x3();
RandomizeEntityState(toAdd);
Space.Add(toAdd);
}
Space.ForceUpdater.Gravity = new Vector3();
game.Camera.Position = new Vector3(0, 6, 15);
}
public RigidBody Create(TGCVector3 position, float radius, float height)
{
var mass = 60f;
var capsule = new CapsuleShape(radius, height);
return(CreateRigidBody(position, mass, capsule));
}
/// <summary>
/// 剛体を生成する
/// </summary>
/// <param name="rigidBodyData_s">剛体データ</param>
private void CreateRigid(List <RigidBodyData> rigidBodyData_s)
{
foreach (var r in rigidBodyData_s)
{
var tempRigidBodyData = new TempRigidBodyData(r);
var init_matrix = tempRigidBodyData.init_matrix;
tempRigidBodyData_s.Add(tempRigidBodyData);
BulletSharp.RigidBody rigidBody = null;
CollisionShape collisionShape;
switch (r.Shape)
{
case RigidBodyShape.Sphere: // 球体
collisionShape = new SphereShape(r.Size.X);
break;
case RigidBodyShape.Box: // ボックス
collisionShape = new BoxShape(r.Size.X, r.Size.Y, r.Size.Z);
break;
case RigidBodyShape.Capsule: // カプセル
collisionShape = new CapsuleShape(r.Size.X, r.Size.Y);
break;
default: // 例外処理
throw new System.Exception("Invalid rigid body data");
}
var rigidProperty = new RigidProperty(r.Mass, r.Repulsion, r.Friction, r.MoveAttenuation, r.RotationAttenuation);
var superProperty = new SuperProperty(r.PhysicsCalcType == PhysicsCalcType.Static, (CollisionFilterGroups)(1 << r.RigidBodyGroup), (CollisionFilterGroups)r.UnCollisionGroupFlag);
rigidBody = bulletManager.CreateRigidBody(collisionShape, init_matrix, rigidProperty, superProperty);
rigidBodies.Add(rigidBody);
}
}
public void Test_New()
{
var cap = new CapsuleShape (1, 2);
Assert.AreEqual (1, cap.Radius);
Assert.AreEqual (2, cap.HalfHeight);
}
CapsuleShape _CreateCapsuleShape()
{
CapsuleShape cs = null;
/*
* if (upAxis == Axis.x)
* {
* cs = new CapsuleShapeX(radius, height);
* }
* else if (upAxis == Axis.y)
* {
* cs = new CapsuleShape(radius, height);
* }
* else if (upAxis == Axis.z)
* {
* cs = new CapsuleShapeZ(radius, height);
* }
* else
* {
* Debug.LogError("invalid axis value");
* }
*/
cs = new CapsuleShape(radius, height);
cs.LocalScaling = m_localScaling.ToBullet();
cs.Margin = m_Margin;
return(cs);
}
protected override void RebuildRigidBody()
{
lock (PhysicsSimulation.Locker)
{
World?.Physics.World.RemoveRigidBody(RigidBody);
_capsuleShape?.Dispose();
RigidBody?.Dispose();
_capsuleShape = new CapsuleShape(_capsuleRadius, _capsuleHeight / 2);
var mass = GetMass();
var intertia = _capsuleShape.CalculateLocalInertia(mass);
var constructionInfo = new RigidBodyConstructionInfo(mass, _motionState, _capsuleShape, intertia);
RigidBody = new RigidBody(constructionInfo)
{
AngularFactor = new BulletSharp.Math.Vector3(0, 0, 0),
CollisionFlags = CollisionFlags.CharacterObject,
ActivationState = ActivationState.DisableDeactivation,
UserObject = this
};
RigidBody.UpdateInertiaTensor();
World?.Physics.World.AddRigidBody(RigidBody);
}
}
public CapsuleCollider()
{
NotifyColliderChanged(
new Entity <ConvexCollidable <CapsuleShape> >(
new ConvexCollidable <CapsuleShape>(
shape = new CapsuleShape(1, 1))));
}
public RigidBodyBone(RigidContainer rcon)
{
bodyContainer = rcon;
CollisionShape shape = null;
switch (rcon.PrimitiveType)
{
case PhysPrimitiveType.Box:
shape = new BoxShape(rcon.Size.Convert());
break;
case PhysPrimitiveType.Capsule:
shape = new CapsuleShape(rcon.Size.X, rcon.Size.Y);
break;
case PhysPrimitiveType.Sphere:
shape = new SphereShape(rcon.Size.X);
break;
}
if (rcon.Phys == PhysType.FollowBone)
{
rcon.Mass = 0;
}
bool isDynamic = (rcon.Mass != 0.0f);
Vector3 inertia = Vector3.Zero;
if (isDynamic)
{
shape.CalculateLocalInertia(rcon.Mass, out inertia);
}
startTransform = Matrix.RotationYawPitchRoll(rcon.Rotation.Y, rcon.Rotation.X, rcon.Rotation.Z) * Matrix.Translation(rcon.Position.Convert());
//Convert left to right coordinates
var reverce = Matrix.Scaling(new Vector3(1, 1, -1));
startTransform = reverce * startTransform * reverce;
rbInfo = new RigidBodyConstructionInfo(rcon.Mass, new DefaultMotionState(startTransform), shape, inertia);
Body = new RigidBody(rbInfo);
Body.ActivationState = ActivationState.DisableDeactivation;
Body.Friction = rcon.Friction;
Body.SetDamping(rcon.MassAttenuation, rcon.RotationDamping);
Body.Restitution = rcon.Restitution;
if (rcon.Phys == PhysType.FollowBone)
{
//Body.SetCustomDebugColor(new Vector3(0, 1, 0));
Body.CollisionFlags = Body.CollisionFlags | CollisionFlags.KinematicObject;
}
/*
* else if (rcon.Phys == PhysType.Gravity)
* Body.SetCustomDebugColor(new Vector3(1, 0, 0));
* else if (rcon.Phys == PhysType.GravityBone)
* Body.SetCustomDebugColor(new Vector3(0, 0, 1));
*/
//Disabled debug color cause its freeze engine on 3rd model load
}
private bool VerifyCollision(Vector3 difference, CapsuleShapeX sharkBody, CapsuleShape cameraBody)
{
var epsilon = this.Mesh.BoundingBox.PMax - this.Mesh.BoundingBox.PMin;
return(FastMath.Pow2(difference.X) <= FastMath.Pow2(sharkBody.Radius + sharkBody.HalfHeight - cameraBody.Radius) * epsilon.X &&
FastMath.Pow2(difference.Y) <= FastMath.Pow2(sharkBody.Radius - (cameraBody.Radius + cameraBody.HalfHeight)) * epsilon.Y &&
FastMath.Pow2(difference.Z) <= FastMath.Pow2(sharkBody.Radius - cameraBody.Radius) * epsilon.Z);
}
public void Test_CreateBulletObject()
{
var cap = new CapsuleShape (1, 2);
Assert.IsNotNull (cap.CreateGhostObject ());
Assert.IsNotNull (cap.CreateRigidBody (1));
Assert.IsNotNull (cap.CreateBulletShape ());
}
public void GetMesh()
{
var s = new CapsuleShape(3, 10);
var mesh = s.GetMesh(0.05f, 3);
Assert.Greater(mesh.NumberOfTriangles, 1);
// No more tests necessary because we see the result in the samples.
}
public override void Init()
{
base.Init();
Vector3 worldScale = WorldScale;
m_scaledShape = new CapsuleShape(worldScale.Y * Length, Math.Abs(Math.Max(worldScale.X, worldScale.Z)) * Radius);
m_scaledStaticCollidable = m_scaledShape.GetCollidableInstance();
}
public override CollisionShape GetCollisionShape()
{
if (collisionShapePtr == null)
{
collisionShapePtr = new CapsuleShape(radius, height);
}
return(collisionShapePtr);
}
/// <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, Vector3?offset = null, Quaternion?localrot = null)
{
Type = ColliderShapeTypes.Capsule;
Is2D = is2D;
Length = length;
Radius = radius;
Orientation = orientation;
Matrix rotation;
CapsuleShape shape;
cachedScaling = Is2D ? new Vector3(1, 1, 0) : Vector3.One;
switch (orientation)
{
case ShapeOrientation.UpZ:
shape = new CapsuleShapeZ(radius, length)
{
LocalScaling = cachedScaling,
};
rotation = Matrix.RotationX((float)Math.PI / 2.0f);
break;
case ShapeOrientation.UpY:
shape = new CapsuleShape(radius, length)
{
LocalScaling = cachedScaling,
};
rotation = Matrix.Identity;
break;
case ShapeOrientation.UpX:
shape = new CapsuleShapeX(radius, length)
{
LocalScaling = cachedScaling,
};
rotation = Matrix.RotationZ((float)Math.PI / 2.0f);
break;
default:
throw new ArgumentOutOfRangeException("orientation");
}
InternalShape = Is2D ? (CollisionShape) new Convex2DShape(shape)
{
LocalScaling = cachedScaling
} : shape;
DebugPrimitiveMatrix = Matrix.Scaling(new Vector3(DebugScaling)) * rotation;
if (offset.HasValue || localrot.HasValue)
{
LocalOffset = offset ?? Vector3.Zero;
LocalRotation = localrot ?? Quaternion.Identity;
UpdateLocalTransformations();
}
}
public LockRotationSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// Next, we add boxes and capsules in random positions and orientations.
// For all bodies the flags LockRotationX/Y/Z are set. This will prevent all
// rotation that would be caused by forces. (It is still allowed to manually
// change the rotation or to set an angular velocity.)
BoxShape boxShape = new BoxShape(0.5f, 0.8f, 1.2f);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-10, 10);
position.Y = 5;
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(boxShape)
{
Pose = new Pose(position, orientation),
LockRotationX = true,
LockRotationY = true,
LockRotationZ = true,
};
Simulation.RigidBodies.Add(body);
}
CapsuleShape capsuleShape = new CapsuleShape(0.3f, 1.2f);
for (int i = 0; i < 10; i++)
{
Vector3F randomPosition = RandomHelper.Random.NextVector3F(-10, 10);
randomPosition.Y = 5;
QuaternionF randomOrientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(capsuleShape)
{
Pose = new Pose(randomPosition, randomOrientation),
LockRotationX = true,
LockRotationY = true,
LockRotationZ = true,
};
Simulation.RigidBodies.Add(body);
}
}
public void GetMesh()
{
var s = new CapsuleShape(3, 10);
var mesh = s.GetMesh(0.05f, 3);
Assert.Greater(mesh.NumberOfTriangles, 1);
// No more tests necessary because we see the result in the samples.
}
public ResponseFilterSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Only disable collision response if you need collision detection info but no collision
// response. If you can disable collision detection too, use
// Simulation.CollisionDomain.CollisionDetection.CollisionFilter
// instead - this is more efficient!
// (In this sample, a custom filter implementation is used. DigitalRune.Physics provides
// a standard filter implementation: DigitalRune.Physics.CollisionResponseFilter.)
Simulation.ResponseFilter = new MyCollisionResponseFilter();
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- Add boxes at random poses.
BoxShape boxShape = new BoxShape(0.5f, 0.8f, 1.2f);
for (int i = 0; i < 20; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
position.Y = 5;
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(boxShape)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(body);
}
// ----- Add capsules at random poses.
CapsuleShape capsuleShape = new CapsuleShape(0.3f, 1.2f);
for (int i = 0; i < 20; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
position.Y = 5;
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(capsuleShape)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(body);
}
}
private void TryZ()
{
if (croutch)
{
return;
}
CapsuleShape maincap = mainBody.Shapes[1] as CapsuleShape;
CapsuleShape downcap = mainBody.Shapes[2] as CapsuleShape;
if (Type.FPSCameraOffset != new Vec3(0, 0, -0.5f))
{
Z = false;
}
else
{
Z = true;
}
if (!Z)
{
Type.Height = Type.HeightMax / 3;
}
else
{
Type.Height = Type.HeightMax;
}
float length2 = Type.Height - Type.Radius * 2 - Type.WalkUpHeight;
if (!Z)
{
Type.WalkMaxVelocity = 2;
Type.WalkForce = 2000f;
Type.FPSCameraOffset = new Vec3(0, 0, -0.5f);
maincap.Position = new Vec3(0, 0, -0.5f);
maincap.Length = length2 / 2;
downcap.Position = new Vec3(0, 0, -0.5f);
downcap.Length = 1.2f;
}
if (Z)
{
Type.WalkMaxVelocity = 7;
Type.WalkForce = 6000f;
Type.FPSCameraOffset = new Vec3(0, 0, 0.5f);
maincap.Length = length2;
maincap.Position = Vec3.Zero;
downcap.Length = Type.Height - Type.BottomRadius * 2;
downcap.Position = new Vec3(0, 0,
(Type.Height - Type.WalkUpHeight) / 2 - Type.Height / 2);
}
}
public void Clone()
{
CapsuleShape capsule = new CapsuleShape(1.23f, 45.6f);
CapsuleShape clone = capsule.Clone() as CapsuleShape;
Assert.IsNotNull(clone);
Assert.AreEqual(capsule.Radius, clone.Radius);
Assert.AreEqual(capsule.Height, clone.Height);
Assert.AreEqual(capsule.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(capsule.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void CapsuleTest()
{
var s = new CapsuleShape(1, 2);
var v0 = s.GetVolume(0.001f, 10);
var m = s.GetMesh(0.001f, 10);
var v1 = m.GetVolume();
Assert.IsTrue(Numeric.AreEqual(v0, v1, 0.01f * (1 + v0))); // 1% error is allowed.
}
void IConstructable.Construct(IDictionary <string, string> param)
{
var type = param["type"];
switch (type)
{
case "trimesh":
var physData = ResourceFactory.LoadAsset <PhysicsData>(param["physData"]);
Shape shape = new TriangleMeshShape(physData.Octree);
Body = new RigidBody(shape)
{
Material = { Restitution = 0f, KineticFriction = 0f }
};
break;
case "hull":
physData = ResourceFactory.LoadAsset <PhysicsData>(param["physData"]);
shape = new ConvexHullShape(physData.Vertices);
Body = new RigidBody(shape);
break;
case "sphere":
shape = new SphereShape(float.Parse(param["radius"], CultureInfo.InvariantCulture));
Body = new RigidBody(shape);
break;
case "box":
var d = param["size"].ConvertToVector();
var offset = param.Get("offset", "0;0;0").ConvertToVector();
shape = new BoxShape(2.0f * d.ToJVector());
Body = new RigidBody(shape)
{
Position = offset.ToJVector()
};
break;
case "capsule":
var height = float.Parse(param["height"], CultureInfo.InvariantCulture);
var radius = float.Parse(param["radius"], CultureInfo.InvariantCulture);
shape = new CapsuleShape(height, radius);
Body = new RigidBody(shape)
{
Position = JVector.Backward * (0.5f * height + radius),
Orientation = JMatrix.CreateRotationX(MathHelper.PiOver2)
};
break;
default:
throw new Exception("Unknown shape: " + type);
}
Body.IsStatic = Convert.ToBoolean(param.Get("static", "false"));
Body.Material.KineticFriction = 0.5f;
Body.Material.StaticFriction = 0.5f;
Body.Material.Restitution = 0.5f;
}
public virtual void LoadPlayerController(Entity playerEntity, SceneNode characterNode, object userData, Vector3 mobNodePositionUpdate)
{
//if (!initialized)
//{
// characterToLoad = characterNode;
// characterEntityToLoad = playerEntity;
// JumpHandlerToLoad = jumpHandler;
// MobNodePositionUpdateToLoad = mobNodePositionUpdate;
// userDataToLoad = userData;
// return;
//}
if (playerController != null)
{
return;
}
float modelHeight = 2f;// (playerEntity.BoundingBox.Max.Y) / scaleFactor; // AJ: used to subtract minimum from maximum- playerEntity.BoundingBox.Minimum.y
System.Console.WriteLine("Player capsule info: modelheight '{0}', boundingbox max '{1}', bounding box min '{2}' and playerPosition '{3}'",
modelHeight, playerEntity.BoundingBox.Max.Y, playerEntity.BoundingBox.Min.Y, characterNode.Position);
float radius = 1.75f;
float height = 1.75f;
ConvexShape capsule = new CapsuleShape(radius, height);
//ConvexShape capsule = new SphereShape(radius);
ghostObject = new PairCachingGhostObject();
Vector3 position = new Vector3(0, 0, 0);//new Vector3(characterNode.Position.X / scaleFactor, (characterNode.Position.Y + 1500) / scaleFactor, characterNode.Position.Z / scaleFactor);
//IndexedMatrix worldTransform = IndexedMatrix.CreateTranslation(characterNode.Position.X / scaleFactor,
// (characterNode.Position.Y + 1500) / scaleFactor, characterNode.Position.Z / scaleFactor);
IndexedMatrix worldTransform = IndexedMatrix.CreateTranslation(position);
ghostObject.SetWorldTransform(worldTransform);
//broadphase.OverlappingPairCache.SetInternalGhostPairCallback(new GhostPairCallback());
ghostObject.SetCollisionShape(capsule);
ghostObject.SetCollisionFlags(CollisionFlags.CF_CHARACTER_OBJECT);
float stepHeight = 0.35f;
playerController = new KinematicCharacterController(ghostObject, capsule, stepHeight, 1);
//characterToLoad = null;
BulletMobState mobMovementState = new BulletMobState(playerController, mobNodePositionUpdate);
//mobMovementState.JumpEvent += jumpHandler;
mobControllers.Add(characterNode, mobMovementState);
//m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
//m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.DefaultFilter, CollisionFilterGroups.AllFilter);
m_dynamicsWorld.AddAction(playerController);
//collisionShapes.Add(capsule);
//frozenTime = 0;
}
public void Test_CreateShape()
{
var box = new CapsuleShape (1, 2);
var shp = box.CreateBulletShape () as BulletSharp.CapsuleShape;
var height = shp.HalfHeight * PhysicsSimulator.PPM;
var radius = shp.Radius * PhysicsSimulator.PPM;
Assert.AreEqual (1, radius);
Assert.AreEqual (1, height);
}
void insertrod()
{
Shape rod = new CapsuleShape(new CapsuleShapeProperties(1000, new Pose(), .008f, 3));
SingleShapeEntity hangrod = new SingleShapeEntity(rod, new Vector3(0, 1, .05f));
hangrod.State.Name = "hangrod";
hangrod.Rotation = new Microsoft.Xna.Framework.Vector3(0, 0, 90);
//hangrod.State.Pose.Orientation = new Quaternion(0, 0, 1, 1);
hangrod.State.Flags = EntitySimulationModifiers.Kinematic;
SimulationEngine.GlobalInstancePort.Insert(hangrod);
}
public void Clone()
{
CapsuleShape capsule = new CapsuleShape(1.23f, 45.6f);
CapsuleShape clone = capsule.Clone() as CapsuleShape;
Assert.IsNotNull(clone);
Assert.AreEqual(capsule.Radius, clone.Radius);
Assert.AreEqual(capsule.Height, clone.Height);
Assert.AreEqual(capsule.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(capsule.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void AddRigidBody(Physics3DRigidBody rb, Components.RigidBodyDesc desc)
{
MotionState motionState;
Matrix4x4 mat = MatrixExt.Transform(desc.Position, desc.Rotation);
rb.defaultPosition = desc.Position;
rb.defaultRotation = desc.Rotation;
motionState = new DefaultMotionState(GetMatrix(mat));
CollisionShape collisionShape;
switch (desc.Shape)
{
case Components.RigidBodyShape.Sphere:
collisionShape = new SphereShape(desc.Dimemsions.X);
break;
case Components.RigidBodyShape.Capsule:
collisionShape = new CapsuleShape(desc.Dimemsions.X, desc.Dimemsions.Y);
break;
case Components.RigidBodyShape.Box:
default:
collisionShape = new BoxShape(GetVector3(desc.Dimemsions));
break;
}
float mass = desc.Mass;
BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
if (desc.Type == 0)
{
mass = 0;
}
else
{
collisionShape.CalculateLocalInertia(mass, out localInertia);
}
var rigidbodyInfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
rigidbodyInfo.Friction = desc.Friction;
rigidbodyInfo.LinearDamping = desc.LinearDamping;
rigidbodyInfo.AngularDamping = desc.AngularDamping;
rigidbodyInfo.Restitution = desc.Restitution;
rb.rigidBody = new RigidBody(rigidbodyInfo);
rb.rigidBody.ActivationState = ActivationState.DisableDeactivation;
rb.rigidBody.SetSleepingThresholds(0, 0);
if (desc.Type == Components.RigidBodyType.Kinematic)
{
rb.rigidBody.CollisionFlags |= CollisionFlags.KinematicObject;
}
world.AddRigidBody(rb.rigidBody, 1 << desc.CollisionGroup, desc.CollisionMask);
}
//CapsuleShape
public ICapsuleShapeImp AddCapsuleShape(float radius, float height)
{
var btCapsuleShape = new CapsuleShape(radius, height);
BtCollisionShapes.Add(btCapsuleShape);
var retval = new CapsuleShapeImp();
retval.BtCapsuleShape = btCapsuleShape;
btCapsuleShape.UserObject = retval;
return(retval);
}
//Get Collision shape
public override CollisionShape GetCollisionShape()
{
if (Shape != null)
{
return(Shape);
}
Shape = new CapsuleShape(_radius, _height)
{
LocalScaling = transform.localScale.ToBullet()
};
return(Shape);
}
private static RigidBody CreateRigidBody(TGCVector3 position, float mass, CapsuleShape capsule)
{
var inertia = capsule.CalculateLocalInertia(mass);
var transform = TGCMatrix.Translation(position);
var motionState = new DefaultMotionState(transform.ToBsMatrix);
var rigidBodyInfo = new RigidBodyConstructionInfo(mass, motionState, capsule, inertia);
return(new RigidBody(rigidBodyInfo)
{
AngularFactor = Vector3.UnitY
});
}
public void TestProperties()
{
CapsuleShape b = new CapsuleShape();
Assert.AreEqual(0, b.Radius);
Assert.AreEqual(0, b.Height);
b.Height = 10;
Assert.AreEqual(10, b.Height);
Assert.AreEqual(0, b.Radius);
b.Radius = 4;
Assert.AreEqual(10, b.Height);
Assert.AreEqual(4, b.Radius);
}
public void HeightException()
{
CapsuleShape b = new CapsuleShape(3, 7);
b.Height = 4; // 2*Radius > Height --> ERROR
}
private RigidBody CreateRigidBody(MMDRigid rigid, MMDModel Model,out short group, out MMDMotionState motionStateStart)
{
CollisionShape collision;
RigidBody body;
//衝突スキンの作成
switch (rigid.ShapeType)
{
case 0:
collision = new SphereShape(rigid.ShapeWidth);
break;
case 1:
collision = new BoxShape(new btVector3(rigid.ShapeWidth, rigid.ShapeHeight, rigid.ShapeDepth));
break;
case 2:
collision = new CapsuleShape(rigid.ShapeWidth, rigid.ShapeHeight);
break;
default:
throw new NotImplementedException("不明な剛体タイプ");
}
motionStateStart = new MMDMotionState(rigid, Model);
btVector3 localInertia = btVector3.Zero;
//イナーシャの計算
if (rigid.Type != 0)
collision.calculateLocalInertia(rigid.Weight, out localInertia);
//剛体を作成
body = new RigidBody((rigid.Type != 0 ? rigid.Weight : 0), motionStateStart, collision, localInertia);
//ダンピング値、摩擦、Restitutionをセット
body.setDamping(rigid.LinerDamping, rigid.AngularDamping);
body.Friction = rigid.Friction;
body.Restitution = rigid.Restitution;
//ボーン追従型はKinematicにする
if (rigid.Type == 0)
{
body.ActivationState |= ActivationStateFlags.DISABLE_DEACTIVATION;
body.CollisionFlags = CollisionFlags.CF_KINEMATIC_OBJECT;
if (!string.IsNullOrEmpty(rigid.RelatedBoneName))
Model.BoneManager[rigid.RelatedBoneName].IsPhysics = false;
}
else
{//物理演算型はボーンのフラグをオンにする
if (!string.IsNullOrEmpty(rigid.RelatedBoneName))
Model.BoneManager[rigid.RelatedBoneName].IsPhysics = true;
}
//グループのフラグをセット
group = (short)Math.Pow(2, rigid.GroupIndex);
return body;
}
public void SerializationBinary()
{
var a = new CapsuleShape(11, 22);
// Serialize object.
var stream = new MemoryStream();
var formatter = new BinaryFormatter();
formatter.Serialize(stream, a);
// Deserialize object.
stream.Position = 0;
var deserializer = new BinaryFormatter();
var b = (CapsuleShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Radius, b.Radius);
Assert.AreEqual(a.Height, b.Height);
}
public void RadiusException()
{
CapsuleShape b = new CapsuleShape();
b.Radius = 1; // Radius > Height --> ERROR
}
public void SerializationXml()
{
var a = new CapsuleShape(11, 22);
// Serialize object.
var stream = new MemoryStream();
var serializer = new XmlSerializer(typeof(Shape));
serializer.Serialize(stream, a);
// Output generated xml. Can be manually checked in output window.
stream.Position = 0;
var xml = new StreamReader(stream).ReadToEnd();
Trace.WriteLine("Serialized Object:\n" + xml);
// Deserialize object.
stream.Position = 0;
var deserializer = new XmlSerializer(typeof(Shape));
var b = (CapsuleShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Radius, b.Radius);
Assert.AreEqual(a.Height, b.Height);
}
/*private void MyTickCallBack(ManifoldPoint cp, CollisionObjectWrapper colobj0wrap, int partid0, int index0, CollisionObjectWrapper colobj1wrap, int partid1, int index1)
{
Debug.WriteLine("MyTickCallBack");
int numManifolds = BtWorld.Dispatcher.NumManifolds;
RigidBodyImp myRb;
//Debug.WriteLine("numManifolds: " + numManifolds);
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
int numContacts = contactManifold.NumContacts;
if (numContacts > 0)
{
CollisionObject obA = (CollisionObject) contactManifold.Body0;
CollisionObject obB = (CollisionObject) contactManifold.Body1;
// Debug.WriteLine(numContacts);
var pnA = obA.UserObject;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
}
}
}
}*/
public IRigidBodyImp AddRigidBody(float mass, float3 worldTransform, float3 orientation, ICollisionShapeImp colShape/*, float3 intertia*/)
{
// Use bullet to do what needs to be done:
var btMatrix = Matrix.RotationX(orientation.x)
* Matrix.RotationY(orientation.y)
* Matrix.RotationZ(orientation.z)
* Matrix.Translation(worldTransform.x, worldTransform.y, worldTransform.z);
var btMotionState = new DefaultMotionState(btMatrix);
var shapeType = colShape.GetType().ToString();
CollisionShape btColShape;
var isStatic = false;
switch (shapeType)
{
//Primitives
case "Fusee.Engine.BoxShapeImp":
var box = (BoxShapeImp) colShape;
var btBoxHalfExtents = Translater.Float3ToBtVector3(box.HalfExtents);
btColShape = new BoxShape(btBoxHalfExtents);
break;
case "Fusee.Engine.CapsuleShapeImp":
var capsule = (CapsuleShapeImp) colShape;
btColShape = new CapsuleShape(capsule.Radius, capsule.HalfHeight);
break;
case "Fusee.Engine.ConeShapeImp":
var cone = (ConeShapeImp) colShape;
btColShape = new ConeShape(cone.Radius, cone.Height);
break;
case "Fusee.Engine.CylinderShapeImp":
var cylinider = (CylinderShapeImp) colShape;
var btCylinderHalfExtents = Translater.Float3ToBtVector3(cylinider.HalfExtents);
btColShape = new CylinderShape(btCylinderHalfExtents);
break;
case "Fusee.Engine.MultiSphereShapeImp":
var multiSphere = (MultiSphereShapeImp) colShape;
var btPositions = new Vector3[multiSphere.SphereCount];
var btRadi = new float[multiSphere.SphereCount];
for (int i = 0; i < multiSphere.SphereCount; i++)
{
var pos = Translater.Float3ToBtVector3(multiSphere.GetSpherePosition(i));
btPositions[i] = pos;
btRadi[i] = multiSphere.GetSphereRadius(i);
}
btColShape = new MultiSphereShape(btPositions, btRadi);
break;
case "Fusee.Engine.SphereShapeImp":
var sphere = (SphereShapeImp) colShape;
var btRadius = sphere.Radius;
btColShape = new SphereShape(btRadius);
break;
//Misc
case "Fusee.Engine.CompoundShapeImp":
var compShape = (CompoundShapeImp) colShape;
btColShape = new CompoundShape(true);
btColShape = compShape.BtCompoundShape;
break;
case "Fusee.Engine.EmptyShapeImp":
btColShape = new EmptyShape();
break;
//Meshes
case "Fusee.Engine.ConvexHullShapeImp":
var convHull = (ConvexHullShapeImp) colShape;
var btPoints= new Vector3[convHull.GetNumPoints()];
for (int i = 0; i < convHull.GetNumPoints(); i++)
{
var point = convHull.GetScaledPoint(i);
btPoints[i] = Translater.Float3ToBtVector3(point);
}
btColShape = new ConvexHullShape(btPoints);
//btColShape.LocalScaling = new Vector3(3,3,3);
break;
case "Fusee.Engine.StaticPlaneShapeImp":
var staticPlane = (StaticPlaneShapeImp) colShape;
Debug.WriteLine("staticplane: " + staticPlane.Margin);
var btNormal = Translater.Float3ToBtVector3(staticPlane.PlaneNormal);
btColShape = new StaticPlaneShape(btNormal, staticPlane.PlaneConstant);
isStatic = true;
//btColShape.Margin = 0.04f;
//Debug.WriteLine("btColshape" + btColShape.Margin);
break;
case "Fusee.Engine.GImpactMeshShapeImp":
var gImpMesh = (GImpactMeshShapeImp)colShape;
gImpMesh.BtGImpactMeshShape.UpdateBound();
var btGimp = new GImpactMeshShape(gImpMesh.BtGImpactMeshShape.MeshInterface);
btGimp.UpdateBound();
btColShape = btGimp;
break;
//Default
default:
Debug.WriteLine("defaultImp");
btColShape = new EmptyShape();
break;
}
var btLocalInertia = btColShape.CalculateLocalInertia(mass);
// btLocalInertia *= (10.0f*10);
RigidBodyConstructionInfo btRbcInfo = new RigidBodyConstructionInfo(mass, btMotionState, btColShape,
btLocalInertia);
var btRigidBody = new RigidBody(btRbcInfo);
btRigidBody.Restitution = 0.2f;
btRigidBody.Friction = 0.2f;
btRigidBody.CollisionFlags = CollisionFlags.CustomMaterialCallback;
BtWorld.AddRigidBody(btRigidBody);
btRbcInfo.Dispose();
var retval = new RigidBodyImp();
retval._rbi = btRigidBody;
btRigidBody.UserObject = retval;
return retval;
}
public void AddChildShape(float4x4 localTransform, ICapsuleShapeImp shape)
{
var btChildShape = new CapsuleShape(shape.Radius, shape.HalfHeight);
var btLocalTransform = Translater.Float4X4ToBtMatrix(localTransform);
BtCompoundShape.AddChildShape(btLocalTransform, btChildShape);
}
Capsule GetWorldCapsule( CapsuleShape shape )
{
Capsule capsule = new Capsule();
Vec3 pos = shape.Body.Position + shape.Body.Rotation * shape.Position;
Quat rot = shape.Body.Rotation * shape.Rotation;
Vec3 diff = rot * new Vec3( 0, 0, shape.Length * .5f );
capsule.Point1 = pos - diff;
capsule.Point2 = pos + diff;
capsule.Radius = shape.Radius;
return capsule;
}
public void ComputeBoxForCapsules()
{
Vector3F box;
Pose pose;
var points = new CapsuleShape(1, 3).GetMesh(0.01f, 4).Vertices;
Pose cubePose = new Pose(new Vector3F(10, 2, 3), RandomHelper.Random.NextQuaternionF());
for (int i = 0; i < points.Count; i++)
points[i] = cubePose.ToWorldPosition(points[i]);
GeometryHelper.ComputeBoundingBox(points, out box, out pose);
Assert.IsTrue(Numeric.AreEqual(box.LargestComponent, 3, 0.2f));
Assert.IsTrue(Numeric.AreEqual(box.SmallestComponent, 2, 0.2f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(10, 2, 3), pose.Position));
}
//CapsuleShape
public ICapsuleShapeImp AddCapsuleShape(float radius, float height)
{
var btCapsuleShape = new CapsuleShape(radius, height);
BtCollisionShapes.Add(btCapsuleShape);
var retval = new CapsuleShapeImp();
retval.BtCapsuleShape = btCapsuleShape;
btCapsuleShape.UserObject = retval;
return retval;
}
unsafe void CreateCapsuleShape( CapsuleShape shape, float totalVolume )
{
Vec3 position = shape.Position;
Mat3 rotationMatrix = shape.Rotation.ToMat3() * Mat3.FromRotateByY( MathFunctions.PI / 2 );
Quat rotation = rotationMatrix.ToQuat();
float mass = GetShapeMass( shape, totalVolume );
IntPtr material = CreateMaterial( shape );
PhysXNativeBody.CreateCapsuleShape( nativeBody, ref position, ref rotation, shape.Radius, shape.Length * .5f,
1, (IntPtr*)&material, mass, shape.ContactGroup );
}
