public void Cast(CollisionWorld cw, float frameDelta)
{
#if BATCH_RAYCASTER
if (!gBatchRaycaster)
return;
gBatchRaycaster->clearRays ();
for (int i = 0; i < NumRays; i++)
{
gBatchRaycaster->addRay (source[i], dest[i]);
}
gBatchRaycaster->performBatchRaycast ();
for (int i = 0; i < gBatchRaycaster->getNumRays(); i++)
{
const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
_hitPoint[i].setInterpolate3(_source[i], _destination[i], out.HitFraction);
_normal[i] = out.hitNormal;
_normal[i].Normalize();
}
#else
for (int i = 0; i < NumRays; i++)
{
using (var cb = new ClosestRayResultCallback(ref _source[i], ref _destination[i]))
{
cw.RayTestRef(ref _source[i], ref _destination[i], cb);
if (cb.HasHit)
{
_hitPoint[i] = cb.HitPointWorld;
_normal[i] = cb.HitNormalWorld;
_normal[i].Normalize();
}
else
{
_hitPoint[i] = _destination[i];
_normal[i] = new Vector3(1.0f, 0.0f, 0.0f);
}
}
}
_time += frameDelta;
_frameCount++;
if (_frameCount > 50)
{
if (_time < _timeMin) _timeMin = _time;
if (_time > _timeMax) _timeMax = _time;
_timeTotal += _time;
_sampleCount++;
float timeMean = _timeTotal / _sampleCount;
Console.WriteLine("{0} rays in {1} s, min {2}, max {3}, mean {4}",
NumRays * _frameCount,
_time.ToString("0.000", CultureInfo.InvariantCulture),
_timeMin.ToString("0.000", CultureInfo.InvariantCulture),
_timeMax.ToString("0.000", CultureInfo.InvariantCulture),
timeMean.ToString("0.000", CultureInfo.InvariantCulture));
_time = 0;
_frameCount = 0;
}
#endif
}
/// <summary>
/// コンストラクター
/// </summary>
public CollisionAnalyzer()
{
var cc = new DefaultCollisionConfiguration ();
dispatcher = new CollisionDispatcher (cc);
broadphase = new DbvtBroadphase ();
broadphase.OverlappingPairCache.SetInternalGhostPairCallback (new GhostPairCallback ());
// solver = new SequentialImpulseConstraintSolver ();
this.wld = new DiscreteDynamicsWorld (dispatcher, broadphase, null, cc);
this.prevContacts = new List<OverlappingPair> ();
this.currContacts = new List<OverlappingPair> ();
}
public void Cast(CollisionWorld cw, float frameDelta)
{
using (var cb = new ClosestConvexResultCallback())
{
for (int i = 0; i < NumRays; i++)
{
cb.ClosestHitFraction = 1.0f;
cb.ConvexFromWorld = _source[i];
cb.ConvexToWorld = _destination[i];
Matrix from = _fromRotation * Matrix.Translation(_source[i]);
Matrix to = _toRotation * Matrix.Translation(_destination[i]);
cw.ConvexSweepTestRef(_boxShape, ref from, ref to, cb);
if (cb.HasHit)
{
_hitPoint[i] = cb.HitPointWorld;
Vector3.Lerp(ref _source[i], ref _destination[i], cb.ClosestHitFraction, out _hitCenterOfMass[i]);
_hitFraction[i] = cb.ClosestHitFraction;
_normal[i] = cb.HitNormalWorld;
_normal[i].Normalize();
}
else
{
_hitCenterOfMass[i] = _destination[i];
_hitPoint[i] = _destination[i];
_hitFraction[i] = 1.0f;
_normal[i] = new Vector3(1.0f, 0.0f, 0.0f);
}
}
}
_time += frameDelta;
_frameCount++;
if (_frameCount > 50)
{
if (_time < _timeMin) _timeMin = _time;
if (_time > _timeMax) _timeMax = _time;
_timeTotal += _time;
_sampleCount++;
float timeMean = _timeTotal / _sampleCount;
Console.WriteLine("{0} rays in {1} s, min {2}, max {3}, mean {4}",
NumRays * _frameCount,
_time.ToString("0.000", CultureInfo.InvariantCulture),
_timeMin.ToString("0.000", CultureInfo.InvariantCulture),
_timeMax.ToString("0.000", CultureInfo.InvariantCulture),
timeMean.ToString("0.000", CultureInfo.InvariantCulture));
_time = 0;
_frameCount = 0;
}
}
static void Main(string[] args)
{
var config = new BulletSharp.DefaultCollisionConfiguration();
var dispatcher = new BulletSharp.CollisionDispatcher(config);
var pair = new BulletSharp.DbvtBroadphase();
var world = new BulletSharp.CollisionWorld(dispatcher, pair, config);
var players = new Dictionary <Guid, RigidBody>();
var ci = new RigidBodyConstructionInfo(1, new DefaultMotionState(), new SphereShape(.513037f));
var rb = new RigidBody(ci);
rb.Gravity = Vector3.Zero;
rb.Translate(new Vector3(0f, 1.5f, 0f));
world.AddCollisionObject(rb);
}
void OnGround(Kart kart, CollisionWorld.ClosestRayResultCallback callback)
{
// we need to wait two "ticks" of the ray casting (0.1s total at time of writing) before we stop the emitter again
// we could just wait one, but that's too short
// the little boolean stuff is just a sort of "toggle" between the two ticks
// we can't really unhook from the events because we've got multiple karts
bool wait;
if (waitDict.TryGetValue(kart, out wait)) {
if (!wait)
particles[kart].Emitting = false;
else
waitDict[kart] = true;
}
else
waitDict[kart] = true;
}
/*
protected override void Dispose(bool disposing)
{
if (disposing)
{
if (vertexBuffer != null)
{
vertexBuffer.Dispose();
vertexBuffer = null;
}
}
base.Dispose(disposing);
}
*/
public void DrawDebugWorld(CollisionWorld world)
{
world.DebugDrawWorld();
if (lines.Count == 0)
return;
inputAssembler.InputLayout = inputLayout;
if (lineArray.Length != lines.Count)
{
lineArray = new PositionColored[lines.Count];
lines.CopyTo(lineArray);
if (vertexBuffer != null)
{
vertexBuffer.Dispose();
}
vertexBufferDesc.SizeInBytes = PositionColored.Stride * lines.Count;
using (var data = new DataStream(vertexBufferDesc.SizeInBytes, false, true))
{
data.WriteRange(lineArray);
data.Position = 0;
vertexBuffer = new Buffer(device, data, vertexBufferDesc);
}
vertexBufferBinding.Buffer = vertexBuffer;
}
else
{
lines.CopyTo(lineArray);
using (var map = vertexBuffer.Map(MapMode.WriteDiscard))
{
map.WriteRange(lineArray);
}
vertexBuffer.Unmap();
}
inputAssembler.SetVertexBuffers(0, vertexBufferBinding);
inputAssembler.PrimitiveTopology = global::SharpDX.Direct3D.PrimitiveTopology.LineList;
device.Draw(lines.Count, 0);
lines.Clear();
}
public void OnPhysicsStep(CollisionWorld world)
{
Dispatcher dispatcher = world.Dispatcher;
int numManifolds = dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = dispatcher.GetManifoldByIndexInternal(i);
CollisionObject a = contactManifold.Body0;
CollisionObject b = contactManifold.Body1;
if (a is CollisionObject && a.UserObject is BCollisionObject && ((BCollisionObject)a.UserObject).collisionCallbackEventHandler != null)
{
((BCollisionObject)a.UserObject).collisionCallbackEventHandler.OnVisitPersistentManifold(contactManifold);
}
if (b is CollisionObject && b.UserObject is BCollisionObject && ((BCollisionObject)b.UserObject).collisionCallbackEventHandler != null)
{
((BCollisionObject)b.UserObject).collisionCallbackEventHandler.OnVisitPersistentManifold(contactManifold);
}
}
foreach (BCollisionObject.BICollisionCallbackEventHandler coeh in collisionCallbackListeners)
{
if (coeh != null) coeh.OnFinishedVisitingManifolds();
}
}
public void PreStep(CollisionWorld collisionWorld)
{
int numPenetrationLoops = 0;
m_touchingContact = false;
while (RecoverFromPenetration(collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
// printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_currentPosition = m_ghostObject.WorldTransform.Origin;
m_targetPosition = m_currentPosition;
}
public void Reset(CollisionWorld collisionWorld)
{
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection = Vector3.Zero;
m_velocityTimeInterval = 0.0f;
//clear pair cache
HashedOverlappingPairCache cache = m_ghostObject.OverlappingPairCache;
while (cache.OverlappingPairArray.Count > 0)
{
cache.RemoveOverlappingPair(cache.OverlappingPairArray[0].Proxy0, cache.OverlappingPairArray[0].Proxy1, collisionWorld.Dispatcher);
}
}
///btActionInterface interface
public virtual void UpdateAction(CollisionWorld collisionWorld, float deltaTime)
{
PreStep(collisionWorld);
PlayerStep(collisionWorld, deltaTime);
}
protected void StepDown(CollisionWorld collisionWorld, float dt)
{
Matrix start, end, end_double;
bool runonce = false;
// phase 3: down
/*float additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
float downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
Vector3 orig_position = m_targetPosition;
float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
if (downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
{
downVelocity = m_fallSpeed;
}
Vector3 step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine);
callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback2.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
while (true)
{
start = Matrix.Translation(m_currentPosition);
end = Matrix.Translation(m_targetPosition);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double = Matrix.Translation(m_targetPosition - step_drop);
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit)
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
else
{
// this works....
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit)
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
bool has_hit = false;
if (bounce_fix == true)
has_hit = callback.HasHit || callback2.HasHit;
else
has_hit = callback2.HasHit;
if (downVelocity2 > 0.0f && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
Vector3 step_drop2 = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop2;
runonce = true;
continue; //re-run previous tests
}
break;
}
if (callback.HasHit || runonce == true)
{
// we dropped a fraction of the height -> hit floor
float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
if (bounce_fix == true)
{
if (full_drop == true)
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
else
{
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, fraction, out m_currentPosition);
}
}
else
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
full_drop = false;
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
m_wasJumping = false;
}
else
{
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
{
downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
//printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
m_currentPosition = m_targetPosition;
}
}
/// <summary>
/// update the particle emission rates depending on speed
/// </summary>
void OnGround(Kart kart, CollisionWorld.ClosestRayResultCallback callback)
{
float speed = System.Math.Abs(kart.VehicleSpeed);
// if the kart is moving slowly, then just turn the particles completely off
if (speed < 6) {
if (kartSpeedStates[kart.OwnerID] != KartSpeedState.Slow) {
// update this if we need to
kartSpeedStates[kart.OwnerID] = KartSpeedState.Slow;
BothEmitting(kart.OwnerID, false);
}
}
// if we're moving at a medium speed
else if (speed >= 6 && speed < 30) {
if (kartSpeedStates[kart.OwnerID] != KartSpeedState.Medium) {
// update this if we need to
kartSpeedStates[kart.OwnerID] = KartSpeedState.Medium;
if (callback.CollisionObject == dirtBody)
DirtEmitting(kart.OwnerID, true);
else if (callback.CollisionObject == grassBody)
GrassEmitting(kart.OwnerID, true);
}
// make some new emission rates
float dustEmissionRate = (speed / 30) * defaultDustEmissionRate;
float grassEmissionRate = (speed / 30) * defaultGrassEmissionRate;
float mudEmissionRate = (speed / 30) * defaultMudEmissionRate;
// and update the particles
Pair<WheelHelper, WheelHelper> pair = wheelHelpers[kart.OwnerID];
pair.first.dust.GetEmitter(0).EmissionRate = dustEmissionRate;
pair.second.dust.GetEmitter(0).EmissionRate = dustEmissionRate;
pair.first.grass.GetEmitter(0).EmissionRate = grassEmissionRate;
pair.second.grass.GetEmitter(0).EmissionRate = grassEmissionRate;
pair.first.mud.GetEmitter(0).EmissionRate = mudEmissionRate;
pair.second.mud.GetEmitter(0).EmissionRate = mudEmissionRate;
}
// and if we're moving at a fast speed
else {
if (kartSpeedStates[kart.OwnerID] != KartSpeedState.Fast) {
kartSpeedStates[kart.OwnerID] = KartSpeedState.Fast;
// and update the particles
Pair<WheelHelper, WheelHelper> pair = wheelHelpers[kart.OwnerID];
pair.first.dust.GetEmitter(0).EmissionRate = defaultDustEmissionRate;
pair.second.dust.GetEmitter(0).EmissionRate = defaultDustEmissionRate;
pair.first.grass.GetEmitter(0).EmissionRate = defaultGrassEmissionRate;
pair.second.grass.GetEmitter(0).EmissionRate = defaultGrassEmissionRate;
pair.first.mud.GetEmitter(0).EmissionRate = defaultMudEmissionRate;
pair.second.mud.GetEmitter(0).EmissionRate = defaultMudEmissionRate;
}
}
}
public void Cast(CollisionWorld cw)
{
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
using (var cb = new ClosestConvexResultCallback(ref source[i], ref dest[i]))
{
Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f);
Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f);
Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]);
Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]);
cw.ConvexSweepTestRef(boxShape, ref from, ref to, cb);
if (cb.HasHit)
{
hit_surface[i] = cb.HitPointWorld;
hit_com[i] = Vector3.Lerp(source[i], dest[i], cb.ClosestHitFraction);
hit_fraction[i] = cb.ClosestHitFraction;
normal[i] = cb.HitNormalWorld;
normal[i].Normalize();
}
else
{
hit_com[i] = dest[i];
hit_surface[i] = dest[i];
hit_fraction[i] = 1.0f;
normal[i] = new Vector3(1.0f, 0.0f, 0.0f);
}
}
}
frame_counter++;
if (frame_counter > 50)
{
min_ms = ms < min_ms ? ms : min_ms;
max_ms = ms > max_ms ? ms : max_ms;
sum_ms += ms;
sum_ms_samples++;
float mean_ms = (float)sum_ms / (float)sum_ms_samples;
Console.WriteLine("{0} rays in {1} ms {2} {3} {4}", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
}
/*
Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation.
*/
public bool CreatePhysicsWorld( out CollisionWorld world,
out CollisionConfiguration collisionConfig,
out CollisionDispatcher dispatcher,
out BroadphaseInterface broadphase,
out SequentialImpulseConstraintSolver solver,
out SoftBodyWorldInfo softBodyWorldInfo)
{
bool success = true;
if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf)
{
BDebug.LogError(debugType, "For World Type = SoftBodyAndRigidBody collisionType must be collisionType=SoftBodyRigidBodyCollisionConf. Switching");
m_collisionType = CollisionConfType.SoftBodyRigidBodyCollisionConf;
success = false;
}
collisionConfig = null;
if (m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf)
{
collisionConfig = new DefaultCollisionConfiguration();
}
else if (m_collisionType == CollisionConfType.SoftBodyRigidBodyCollisionConf)
{
collisionConfig = new SoftBodyRigidBodyCollisionConfiguration();
}
dispatcher = new CollisionDispatcher(collisionConfig);
if (m_broadphaseType == BroadphaseType.DynamicAABBBroadphase)
{
broadphase = new DbvtBroadphase();
}
else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase)
{
broadphase = new AxisSweep3(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies);
}
else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase_32bit)
{
broadphase = new AxisSweep3_32Bit(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies);
}
else
{
broadphase = null;
}
world = null;
softBodyWorldInfo = null;
solver = null;
if (m_worldType == WorldType.CollisionOnly)
{
world = new CollisionWorld(dispatcher, broadphase, collisionConfig);
}
else if (m_worldType == WorldType.RigidBodyDynamics)
{
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConfig);
}
else if (m_worldType == WorldType.MultiBodyWorld)
{
world = new MultiBodyDynamicsWorld(dispatcher, broadphase, null, collisionConfig);
}
else if (m_worldType == WorldType.SoftBodyAndRigidBody)
{
solver = new SequentialImpulseConstraintSolver();
solver.RandSeed = sequentialImpulseConstraintSolverRandomSeed;
softBodyWorldInfo = new SoftBodyWorldInfo
{
AirDensity = 1.2f,
WaterDensity = 0,
WaterOffset = 0,
WaterNormal = BulletSharp.Math.Vector3.Zero,
Gravity = UnityEngine.Physics.gravity.ToBullet(),
Dispatcher = dispatcher,
Broadphase = broadphase
};
softBodyWorldInfo.SparseSdf.Initialize();
world = new SoftRigidDynamicsWorld(dispatcher, broadphase, solver, collisionConfig);
world.DispatchInfo.EnableSpu = true;
softBodyWorldInfo.SparseSdf.Reset();
softBodyWorldInfo.AirDensity = 1.2f;
softBodyWorldInfo.WaterDensity = 0;
softBodyWorldInfo.WaterOffset = 0;
softBodyWorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero;
softBodyWorldInfo.Gravity = m_gravity.ToBullet();
}
if (world is DiscreteDynamicsWorld)
{
((DiscreteDynamicsWorld)world).Gravity = m_gravity.ToBullet();
}
if (_doDebugDraw)
{
DebugDrawUnity db = new DebugDrawUnity();
db.DebugMode = _debugDrawMode;
world.DebugDrawer = db;
}
return success;
}
public void BuildIslands(Dispatcher dispatcher, CollisionWorld colWorld)
{
btSimulationIslandManager_buildIslands(_native, dispatcher._native, colWorld._native);
}
public void FindUnions(Dispatcher dispatcher, CollisionWorld colWorld)
{
btSimulationIslandManager_findUnions(_native, dispatcher._native, colWorld._native);
}
public void BuildAndProcessIslands(Dispatcher dispatcher, CollisionWorld collisionWorld,
IslandCallback callback)
{
btSimulationIslandManager_buildAndProcessIslands(_native, dispatcher._native,
collisionWorld._native, callback._native);
}
/// <summary>
/// turn on the appropriate particles
/// </summary>
void OnTouchdown(Kart kart, CollisionWorld.ClosestRayResultCallback callback)
{
if (kart.VehicleSpeed > 4f || kart.VehicleSpeed < -4f) {
if (callback.CollisionObject == dirtBody)
DirtEmitting(kart.OwnerID, true);
else if (callback.CollisionObject == grassBody)
GrassEmitting(kart.OwnerID, true);
}
else {
DirtEmitting(kart.OwnerID, false);
GrassEmitting(kart.OwnerID, false);
}
}
/// <summary>
/// turn off all particles
/// </summary>
void OnLiftoff(Kart kart, CollisionWorld.ClosestRayResultCallback callback)
{
BothEmitting(kart.OwnerID, false);
}
public void PlayerStep(CollisionWorld collisionWorld, float dt)
{
// quick check...
if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0)
{
// printf("\n");
return; // no motion
}
m_wasOnGround = OnGround;
// Update fall velocity.
m_verticalVelocity -= Gravity * dt;
if (m_verticalVelocity > 0.0f && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if (m_verticalVelocity < 0.0f && System.Math.Abs(m_verticalVelocity) > System.Math.Abs(m_fallSpeed))
{
m_verticalVelocity = -System.Math.Abs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
Matrix xform = m_ghostObject.WorldTransform;
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
StepUp(collisionWorld);
if (m_useWalkDirection)
{
StepForwardAndStrafe(collisionWorld, ref m_walkDirection);
}
else
{
//printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
float dtMoving =
(dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
m_velocityTimeInterval -= dt;
// how far will we move while we are moving?
Vector3 move = m_walkDirection * dtMoving;
// printf(" dtMoving: %f", dtMoving);
// okay, step
StepForwardAndStrafe(collisionWorld, ref move);
}
StepDown(collisionWorld, dt);
xform.Origin = m_currentPosition;
m_ghostObject.WorldTransform = xform;
}
protected void Dispose(bool disposing)
{
if (debugType >= BDebug.DebugType.Debug) Debug.Log("BDynamicsWorld Disposing physics.");
if (lateUpdateHelper != null)
{
lateUpdateHelper.m_ddWorld = null;
lateUpdateHelper.m_world = null;
}
if (m_world != null)
{
//remove/dispose constraints
int i;
if (_ddWorld != null)
{
if (debugType >= BDebug.DebugType.Debug) Debug.LogFormat("Removing Constraints {0}", _ddWorld.NumConstraints);
for (i = _ddWorld.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = _ddWorld.GetConstraint(i);
_ddWorld.RemoveConstraint(constraint);
if (constraint.Userobject is BTypedConstraint) ((BTypedConstraint)constraint.Userobject).m_isInWorld = false;
if (debugType >= BDebug.DebugType.Debug) Debug.LogFormat("Removed Constaint {0}", constraint.Userobject);
constraint.Dispose();
}
}
if (debugType >= BDebug.DebugType.Debug) Debug.LogFormat("Removing Collision Objects {0}", _ddWorld.NumCollisionObjects);
//remove the rigidbodies from the dynamics world and delete them
for (i = m_world.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = m_world.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
Debug.Assert(body.NumConstraintRefs == 0, "Rigid body still had constraints");
body.MotionState.Dispose();
}
m_world.RemoveCollisionObject(obj);
if (obj.UserObject is BCollisionObject) ((BCollisionObject)obj.UserObject).isInWorld = false;
if (debugType >= BDebug.DebugType.Debug) Debug.LogFormat("Removed CollisionObject {0}", obj.UserObject);
obj.Dispose();
}
if (m_world.DebugDrawer != null)
{
if (m_world.DebugDrawer is IDisposable)
{
IDisposable dis = (IDisposable)m_world.DebugDrawer;
dis.Dispose();
}
}
m_world.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
_ddWorld = null;
m_world = null;
}
if (Broadphase != null)
{
Broadphase.Dispose();
Broadphase = null;
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
Dispatcher = null;
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
CollisionConf = null;
}
if (Solver != null)
{
Solver.Dispose();
Solver = null;
}
if (softBodyWorldInfo != null)
{
softBodyWorldInfo.Dispose();
softBodyWorldInfo = null;
}
_isDisposed = true;
singleton = null;
}
protected void StepUp(CollisionWorld collisionWorld)
{
// phase 1: up
Matrix start, end;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
/* FIXME: Handle penetration properly */
start = Matrix.Translation(m_currentPosition + upAxisDirection[m_upAxis] * (m_convexShape.Margin + m_addedMargin));
end = Matrix.Translation(m_targetPosition);
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -upAxisDirection[m_upAxis], 0.7071f);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
else
{
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, 0f);
}
if (callback.HasHit)
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if (Vector3.Dot(callback.HitNormalWorld, upAxisDirection[m_upAxis]) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.ClosestHitFraction;
if (m_interpolateUp)
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
else
{
m_currentPosition = m_targetPosition;
}
}
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
}
else
{
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
public void UpdateAction(CollisionWorld collisionWorld, float deltaTimeStep)
{
Updated = true;
}
public void Cast(CollisionWorld cw)
{
#if BATCH_RAYCASTER
if (!gBatchRaycaster)
return;
gBatchRaycaster->clearRays ();
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
gBatchRaycaster->addRay (source[i], dest[i]);
}
gBatchRaycaster->performBatchRaycast ();
for (int i = 0; i < gBatchRaycaster->getNumRays (); i++)
{
const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
hit[i].setInterpolate3(source[i],dest[i],out.HitFraction);
normal[i] = out.hitNormal;
normal[i].Normalize();
}
#else
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
using (var cb = new ClosestRayResultCallback(ref source[i], ref dest[i]))
{
cw.RayTest(ref source[i], ref dest[i], cb);
if (cb.HasHit)
{
hit[i] = cb.HitPointWorld;
normal[i] = cb.HitNormalWorld;
normal[i].Normalize();
}
else
{
hit[i] = dest[i];
normal[i] = new Vector3(1.0f, 0.0f, 0.0f);
}
}
}
frame_counter++;
if (frame_counter > 50)
{
min_ms = ms < min_ms ? ms : min_ms;
max_ms = ms > max_ms ? ms : max_ms;
sum_ms += ms;
sum_ms_samples++;
float mean_ms = (float)sum_ms / (float)sum_ms_samples;
Console.WriteLine("{0} rays in {1} ms {2} {3} {4}", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
#endif
}
protected bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
Vector3 minAabb, maxAabb;
m_convexShape.GetAabb(m_ghostObject.WorldTransform, out minAabb, out maxAabb);
collisionWorld.Broadphase.SetAabbRef(m_ghostObject.BroadphaseHandle,
ref minAabb,
ref maxAabb,
collisionWorld.Dispatcher);
bool penetration = false;
collisionWorld.Dispatcher.DispatchAllCollisionPairs(m_ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher);
m_currentPosition = m_ghostObject.WorldTransform.Origin;
float maxPen = 0f;
for (int i = 0; i < m_ghostObject.OverlappingPairCache.NumOverlappingPairs; i++)
{
m_manifoldArray.Clear();
BroadphasePair collisionPair = m_ghostObject.OverlappingPairCache.OverlappingPairArray[i];
CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject;
CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject;
if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse))
continue;
if (collisionPair.Algorithm != null)
{
collisionPair.Algorithm.GetAllContactManifolds(m_manifoldArray);
}
for (int j = 0; j < m_manifoldArray.Count; j++)
{
PersistentManifold manifold = m_manifoldArray[j];
float directionSign = manifold.Body0 == m_ghostObject ? -1f : 1f;
for (int p = 0; p < manifold.NumContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
float dist = pt.Distance;
if (dist < 0.0f)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.NormalWorldOnB * directionSign;//??
}
m_currentPosition += pt.NormalWorldOnB * directionSign * dist * 0.2f;
penetration = true;
}
else
{
//printf("touching %f\n", dist);
}
}
//manifold.ClearManifold();
}
}
Matrix newTrans = m_ghostObject.WorldTransform;
newTrans.Origin = m_currentPosition;
m_ghostObject.WorldTransform = newTrans;
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return penetration;
}
public void StoreIslandActivationState(CollisionWorld world)
{
btSimulationIslandManager_storeIslandActivationState(_native, world._native);
}
void OnTouchdown(Kart kart, CollisionWorld.ClosestRayResultCallback callback)
{
particles[kart].Emitting = true;
waitDict[kart] = false;
}
public void UpdateAction(CollisionWorld collisionWorld, float deltaTimeStep)
{
UpdateVehicle(deltaTimeStep);
}
public void UpdateActivationState(CollisionWorld colWorld, Dispatcher dispatcher)
{
btSimulationIslandManager_updateActivationState(_native, colWorld._native,
dispatcher._native);
}
/// <inheritdoc/>
void System.IDisposable.Dispose()
{
if (wld != null) {
foreach (var obj in wld.CollisionObjectArray.ToArray()) {
var col = obj as BulletSharp.CollisionObject;
wld.RemoveCollisionObject (col);
col.Dispose ();
}
wld.Dispose ();
this.wld = null;
}
}
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
{
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
// phase 2: forward and strafe
Matrix start = Matrix.Identity, end = Matrix.Identity;
m_targetPosition = m_currentPosition + walkMove;
float fraction = 1.0f;
float distance2 = (m_currentPosition - m_targetPosition).LengthSquared;
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
float dot;
Vector3.Dot(ref m_normalizedDirection, ref m_touchingNormal, out dot);
if (dot > 0.0f)
{
//interferes with step movement
//UpdateTargetPositionBasedOnCollision(ref m_touchingNormal, 0.0f, 1.0f);
}
}
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0)
{
start.Origin = (m_currentPosition);
end.Origin = (m_targetPosition);
Vector3 sweepDirNegative = m_currentPosition - m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
float margin = m_convexShape.Margin;
m_convexShape.Margin = margin + m_addedMargin;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
else
{
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
m_convexShape.Margin = margin;
fraction -= callback.ClosestHitFraction;
if (callback.HasHit)
{
// we moved only a fraction
float hitDistance = (callback.HitPointWorld - m_currentPosition).Length;
Vector3 hitNormalWorld = callback.HitNormalWorld;
UpdateTargetPositionBasedOnCollision(ref hitNormalWorld, 0f, 1f);
Vector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.LengthSquared;
if (distance2 > MathUtil.SIMD_EPSILON)
{
currentDir.Normalize();
/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
float dot;
Vector3.Dot(ref currentDir, ref m_normalizedDirection, out dot);
if (dot <= 0.0f)
{
break;
}
}
else
{
// printf("currentDir: don't normalize a zero vector\n");
break;
}
}
else
{
// we moved whole way
m_currentPosition = m_targetPosition;
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}