public KinematicCharacterController(PairCachingGhostObject ghostObject, ConvexShape convexShape, float stepHeight, int upAxis = 1)
{
m_upAxis = upAxis;
m_addedMargin = 0.02f;
m_walkDirection = Vector3.Zero;
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_turnAngle = 0f;
m_convexShape = convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0f;
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
Gravity = 9.8f * 3; // 3G acceleration.
m_fallSpeed = 55.0f; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0f; // ?
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
MaxSlope = MathUtil.DegToRadians(45.0f);
m_currentStepOffset = 0;
full_drop = false;
bounce_fix = false;
}
public KinematicCharacterController(PairCachingGhostObject ghostObject, ConvexShape convexShape, float stepHeight, int upAxis = 1)
{
m_upAxis = upAxis;
m_addedMargin = 0.02f;
m_walkDirection = Vector3.Zero;
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_turnAngle = 0f;
m_convexShape = convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0f;
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
Gravity = 9.8f * 3; // 3G acceleration.
m_fallSpeed = 55.0f; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0f; // ?
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
MaxSlope = MathUtil.DegToRadians(45.0f);
m_currentStepOffset = 0;
full_drop = false;
bounce_fix = false;
}
/// <summary>
/// コンストラクター
/// </summary>
public CollisionObject()
{
this.shape = null;
this.ghostObject = null;
this.offset = new Vector3 (0, 0, 0);
this.collideWith = -1;
this.ignoreWith = 0;
}
/// <inheritdoc/>
public override BulletSharp.PairCachingGhostObject CreateGhostObject()
{
var col = new PairCachingGhostObject ();
col.CollisionShape = new BulletSharp.SphereShape(radius);
col.CollisionFlags |= CollisionFlags.NoContactResponse;
return col;
}
internal override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
world.RemoveCollisionObject(this);
}
}
if (transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogError("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent <BCollisionShape>();
if (m_collisionShape == null)
{
Debug.LogError("There was no collision shape component attached to this BRigidBody. " + name);
return(false);
}
CollisionShape cs = m_collisionShape.GetCollisionShape();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
if (m_collisionObject == null)
{
m_collisionObject = new BulletSharp.PairCachingGhostObject();
m_collisionObject.CollisionShape = cs;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
m_collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
else
{
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionShape = cs;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
m_collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
return(true);
}
internal override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
world.RemoveCollisionObject(m_collisionObject);
}
}
if (transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogError("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent<BCollisionShape>();
if (m_collisionShape == null)
{
Debug.LogError("There was no collision shape component attached to this BRigidBody. " + name);
return false;
}
CollisionShape cs = m_collisionShape.GetCollisionShape();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
if (m_collisionObject == null)
{
m_collisionObject = new BulletSharp.PairCachingGhostObject();
m_collisionObject.CollisionShape = cs;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
m_collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
else {
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionShape = cs;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
m_collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
return true;
}
public KinematicCharacterController(PairCachingGhostObject ghostObject, ConvexShape convexShape, float stepHeight, ref Vector3 up)
{
m_ghostObject = ghostObject;
m_jumpAxis = new Vector3(0.0f, 0.0f, 1.0f);
m_addedMargin = 0.02f;
m_useGhostObjectSweepTest = true;
m_convexShape = convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_gravity = 9.8f * 3.0f; // 3G acceleration.
m_fallSpeed = 55.0f; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0f; // ?
m_SetjumpSpeed = m_jumpSpeed;
m_interpolateUp = true;
m_maxPenetrationDepth = 0.2f;
Up = up;
StepHeight = stepHeight;
MaxSlope = MathUtil.DegToRadians(45.0f);
}
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);
}
static void TestGhostObjectPairs(PairCachingGhostObject ghostObject)
{
AlignedManifoldArray manifoldArray = new AlignedManifoldArray();
AlignedBroadphasePairArray pairArray = ghostObject.OverlappingPairCache.OverlappingPairArray;
int numPairs = pairArray.Count;
for (int i = 0; i < numPairs; i++)
{
manifoldArray.Clear();
BroadphasePair pair = pairArray[i];
//unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
BroadphasePair collisionPair = world.PairCache.FindPair(pair.Proxy0, pair.Proxy1);
if (collisionPair == null)
continue;
if (collisionPair.Algorithm != null)
collisionPair.Algorithm.GetAllContactManifolds(manifoldArray);
for (int j = 0; j < manifoldArray.Count; j++)
{
PersistentManifold manifold = manifoldArray[j];
float directionSign = manifold.Body0 == ghostObject ? -1.0f : 1.0f;
for (int p = 0; p < manifold.NumContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
if (pt.Distance < 0.0f)
{
Vector3 ptA = pt.PositionWorldOnA;
Vector3 ptB = pt.PositionWorldOnB;
Vector3 normalOnB = pt.NormalWorldOnB;
/// work here
}
}
}
}
}
static void TestGCCollection()
{
var conf = new DefaultCollisionConfiguration();
var dispatcher = new CollisionDispatcher(conf);
var broadphase = new DbvtBroadphase();
//var broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);
world.Gravity = new Vector3(0, -10, 0);
dispatcher.NearCallback = DispatcherNearCallback;
CreateBody(0.0f, new BoxShape(50, 1, 50), Vector3.Zero);
var dynamicObject = CreateBody(10.0f, new SphereShape(1.0f), new Vector3(2, 2, 0));
var dynamicObject2 = CreateBody(1.0f, new SphereShape(1.0f), new Vector3(0, 2, 0));
var ghostPairCallback = new GhostPairCallback();
broadphase.OverlappingPairCache.SetInternalGhostPairCallback(ghostPairCallback);
AddToDisposeQueue(ghostPairCallback);
ghostPairCallback = null;
var ghostObject = new PairCachingGhostObject();
ghostObject.CollisionShape = new BoxShape(2);
ghostObject.WorldTransform = Matrix.Translation(2,2,0);
world.AddCollisionObject(ghostObject);
var trimesh = new TriangleMesh();
Vector3 v0 = new Vector3(0, 0, 0);
Vector3 v1 = new Vector3(1, 0, 0);
Vector3 v2 = new Vector3(0, 1, 0);
Vector3 v3 = new Vector3(1, 1, 0);
trimesh.AddTriangle(v0, v1, v2);
trimesh.AddTriangle(v1, v3, v2);
var triangleMeshShape = new BvhTriangleMeshShape(trimesh, false);
var triMeshObject = CreateBody(0, triangleMeshShape, new Vector3(20,0,20));
AddToDisposeQueue(triangleMeshShape);
AddToDisposeQueue(trimesh);
AddToDisposeQueue(triMeshObject);
triangleMeshShape = null;
trimesh = null;
AddToDisposeQueue(conf);
AddToDisposeQueue(dispatcher);
AddToDisposeQueue(broadphase);
AddToDisposeQueue(world);
//conf.Dispose();
conf = null;
//dispatcher.Dispose();
dispatcher = null;
//broadphase.Dispose();
broadphase = null;
world.DebugDrawer = new DebugDrawTest();
AddToDisposeQueue(world.DebugDrawer);
world.SetInternalTickCallback(WorldPreTickCallback);
for (int i = 0; i < 600; i++)
{
world.StepSimulation(1.0f / 60.0f);
}
world.DispatchInfo.DebugDraw = new DebugDrawTest2();
AddToDisposeQueue(world.DispatchInfo.DebugDraw);
world.DispatchInfo.DebugDraw = world.DispatchInfo.DebugDraw;
AddToDisposeQueue(world.DispatchInfo.DebugDraw);
world.DispatchInfo.DebugDraw = null;
world.DebugDrawer = null;
world.DebugDrawer = new DebugDrawTest2();
world.StepSimulation(1.0f / 60.0f);
world.DebugDrawWorld();
AddToDisposeQueue(world.DispatchInfo.DebugDraw);
world.DebugDrawer = new DebugDrawTest();
world.DebugDrawWorld();
AddToDisposeQueue(world.DebugDrawer);
world.DebugDrawer = null;
TestContactTest(dynamicObject, dynamicObject2);
TestGhostObjectPairs(ghostObject);
TestRayCast(dynamicObject);
TestTriangleMeshRayCast(triMeshObject);
dynamicObject = null;
dynamicObject2 = null;
triMeshObject = null;
//world.SetInternalTickCallback(null);
world.Dispose();
world = null;
GC.Collect(GC.MaxGeneration, GCCollectionMode.Forced);
GC.WaitForPendingFinalizers();
TestWeakRefs();
disposeQueue.Clear();
}
/// <inheritdoc/>
void System.IDisposable.Dispose()
{
if (ghostObject != null) {
// （注意）
// このメソッドが呼ばれる時にはすでにデタッチされているので
// ここでコリジョン ワールドから削除する事ができない
// 削除せずに Dispose() を呼ぶとワールドのDispose()を呼んだタイミングで
// BulletSharp が不正なメモリアクセスで落ちる。
// 従って忘れずに OnDestroyed() でワールドから取り除いておくこと。
ghostObject.Dispose ();
this.ghostObject = null;
}
}
///<inheritdoc/>
public override BulletSharp.PairCachingGhostObject CreateGhostObject()
{
var col = new PairCachingGhostObject ();
col.CollisionShape = new BulletSharp.BoxShape (halfWidth, halfHeight, halfDepth);
col.CollisionFlags |= CollisionFlags.NoContactResponse;
return col;
}
internal override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
isInWorld = false;
world.RemoveCollisionObject(m_collisionObject);
}
}
if (transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogError("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent<BCollisionShape>();
if (m_collisionShape == null)
{
Debug.LogError("There was no collision shape component attached to this BRigidBody. " + name);
return false;
}
if (!(m_collisionShape.GetCollisionShape() is ConvexShape))
{
Debug.LogError("The CollisionShape on this BCharacterController was not a convex shape. " + name);
return false;
}
m_collisionShape.GetCollisionShape();
if (m_collisionObject == null)
{
m_collisionObject = new PairCachingGhostObject();
m_collisionObject.CollisionShape = m_collisionShape.GetCollisionShape();
m_collisionObject.CollisionFlags = m_collisionFlags;
m_characterController = new KinematicCharacterController((PairCachingGhostObject)m_collisionObject, (ConvexShape)m_collisionShape.GetCollisionShape(), stepHeight, upAxis);
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
//world.world.AddCollisionObject(m_collisionObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
//((DynamicsWorld)world.world).AddAction(m_characterController);
}
else {
m_collisionObject.CollisionShape = m_collisionShape.GetCollisionShape();
m_collisionObject.CollisionFlags = m_collisionFlags;
if (m_characterController != null)
{
world.RemoveAction(m_characterController);
}
m_characterController = new KinematicCharacterController((PairCachingGhostObject)m_collisionObject, (ConvexShape)m_collisionShape.GetCollisionShape(), stepHeight, upAxis);
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
}
return true;
}
public void CreateGhosts()
{
if (Bf.Animations.Model == null || Bf.Animations.Model.MeshCount <= 0)
return;
Bt.ManifoldArray = new AlignedManifoldArray();
Bt.Shapes.Clear();
Bt.GhostObjects.Clear();
Bt.LastCollisions.Clear();
for(var i = 0; i < Bf.BoneTags.Count; i++)
{
var box = COLLISION_GHOST_VOLUME_COEFFICIENT *
(Bf.BoneTags[i].MeshBase.BBMax - Bf.BoneTags[i].MeshBase.BBMin);
Bt.Shapes.Add(new BoxShape(box.ToBullet()));
Bt.Shapes.Last().Margin = COLLISION_MARGIN_DEFAULT;
Bf.BoneTags[i].MeshBase.Radius = Math.Min(Math.Min(box.X, box.Y), box.Z);
var pcg = new PairCachingGhostObject();
pcg.SetIgnoreCollisionCheck(Bt.BtBody[i], true);
var gltr = Transform * Bf.BoneTags[i].FullTransform;
gltr.Origin = gltr * Bf.BoneTags[i].MeshBase.Center;
pcg.WorldTransform = ((Matrix4)gltr).ToBullet();
pcg.CollisionFlags |= CollisionFlags.NoContactResponse | CollisionFlags.CharacterObject;
pcg.UserObject = Self;
pcg.CollisionShape = Bt.Shapes.Last();
BtEngineDynamicsWorld.AddCollisionObject(pcg, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.AllFilter);
Bt.GhostObjects.Add(pcg);
Bt.LastCollisions.Add(new EntityCollisionNode());
}
}
/// <summary>
/// It is from bullet_character_controller
/// </summary>
public static int GhostGetPenetrationFixVector(PairCachingGhostObject ghost, AlignedManifoldArray manifoldArray,
out Vector3 correction)
{
// Here we must refresh the overlapping paircache as the penetrating movement itself or the
// previous recovery iteration might have used setWorldTransform and pushed us into an object
// that is not in the previous cache contents from the last timestep, as will happen if we
// are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck.
//
// Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase
// paircache and the ghostobject's internal paircache at the same time. /BW
var ret = 0;
var pairArray = ghost.OverlappingPairCache.OverlappingPairArray;
BulletSharp.Math.Vector3 aabb_min, aabb_max, t;
ghost.CollisionShape.GetAabb(ghost.WorldTransform, out aabb_min, out aabb_max);
BtEngineDynamicsWorld.Broadphase.SetAabb(ghost.BroadphaseHandle, aabb_min, aabb_max,
BtEngineDynamicsWorld.Dispatcher);
BtEngineDynamicsWorld.Dispatcher.DispatchAllCollisionPairs(ghost.OverlappingPairCache,
BtEngineDynamicsWorld.DispatchInfo, BtEngineDynamicsWorld.Dispatcher);
correction = Vector3.Zero;
var numPairs = ghost.OverlappingPairCache.NumOverlappingPairs;
for (var i = 0; i < numPairs; i++)
{
manifoldArray.Clear();
// do not use commented code: it prevents to collision skips.
//var pair = pairArray[i];
//var collisionPair = Global.BtEngineDynamicsWorld.PairCache.FindPair(pair.Proxy0, pair.Proxy1);
var collisionPair = pairArray[i];
if(collisionPair == null)
{
continue;
}
collisionPair.Algorithm?.GetAllContactManifolds(manifoldArray);
foreach (var manifold in manifoldArray)
{
var directionSign = manifold.Body0 == ghost ? -1.0f : 1.0f;
var cont0 = (EngineContainer) manifold.Body0.UserObject;
var cont1 = (EngineContainer) manifold.Body1.UserObject;
if(cont0.CollisionType == COLLISION_TYPE.Ghost && cont1.CollisionType == COLLISION_TYPE.Ghost)
{
continue;
}
for(var k = 0; k < manifold.NumContacts; k++)
{
var pt = manifold.GetContactPoint(k);
var dist = pt.Distance;
if(dist < 0.0f)
{
t = pt.NormalWorldOnB * dist * directionSign;
correction += t.ToOpenTK();
ret++;
}
}
}
}
return ret;
}
