public static bool CastRayFromMouse(float3 cameraPos, float3 mouseWorldPos, float distance, out Unity.Physics.RaycastHit closestHit, CollisionFilter collisionFilter, CollisionWorld collisionWorld)
{
float3 origin = cameraPos;
float3 direction = math.normalize(mouseWorldPos - origin);
RaycastInput rayInput = new RaycastInput()
{
Start = origin,
End = origin + (direction * distance),
Filter = collisionFilter
};
return(collisionWorld.CastRay(rayInput, out closestHit));
}
protected virtual void _InitializePhysicsWorld()
{
_isDisposed = false;
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;
}
if (m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf)
{
CollisionConf = new DefaultCollisionConfiguration();
}
else if (m_collisionType == CollisionConfType.SoftBodyRigidBodyCollisionConf)
{
CollisionConf = new SoftBodyRigidBodyCollisionConfiguration();
}
Dispatcher = new CollisionDispatcher(CollisionConf);
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;
}
if (m_worldType == WorldType.CollisionOnly)
{
m_world = new CollisionWorld(Dispatcher, Broadphase, CollisionConf);
_ddWorld = null;
}
else if (m_worldType == WorldType.RigidBodyDynamics)
{
m_world = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
_ddWorld = (DiscreteDynamicsWorld)m_world;
}
else if (m_worldType == WorldType.MultiBodyWorld)
{
m_world = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
_ddWorld = (DiscreteDynamicsWorld)m_world;
}
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();
m_world = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
_ddWorld = (DiscreteDynamicsWorld)m_world;
m_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 (_ddWorld != null)
{
_ddWorld.Gravity = m_gravity.ToBullet();
}
if (_doDebugDraw)
{
DebugDrawUnity db = new DebugDrawUnity();
db.DebugMode = _debugDrawMode;
m_world.DebugDrawer = db;
}
}
public void UpdateAction(CollisionWorld collisionWorld, float deltaTimeStep)
{
UpdateVehicle(deltaTimeStep);
}
// Update is called once per frame
protected override void OnUpdate( )
{
PhysicsWorld physicsWorld = buildPhysicsWorldSystem.PhysicsWorld;
CollisionWorld collisionWorld = physicsWorld.CollisionWorld;
// EntityCommandBuffer commandBuffer = m_EntityCommandBufferSystem.CreateCommandBuffer();
// Dependency = JobHandle.CombineDependencies(Dependency, m_EndFramePhysicsSystem.GetOutputDependency());
Dependency = JobHandle.CombineDependencies(Dependency, endFramePhysicsSystem.GetOutputDependency());
/*
* Entities
* .WithName ( "RaycastWithCustomCollectorJob" )
* .WithBurst ()
* .ForEach ( ( Entity entity, ref Translation position, ref Rotation rotation ) =>
* {
*
*
* // var collector = new IgnoreTransparentClosestHitCollector ( collisionWorld ) ;
*
* // collisionWorld.CastRay ( raycastInput, ref collector ) ;
*
* //if ( collisionWorld.Bodies [hit.RigidBodyIndex].Collider, hit.ColliderKey )
* //{
* // return false;
* //}
*
* // var collector = new IgnoreTransparentClosestHitCollector ( collisionWorld ) ;
*
*
* var raycastLength = 200 ;
*
* // Perform the Raycast
* var raycastInput = new RaycastInput
* {
* Start = position.Value,
* End = position.Value + ( math.forward (rotation.Value) * raycastLength ),
* Filter = CollisionFilter.Default
* };
*
* collisionWorld.CastRay(raycastInput, ref collector);
*
* var hit = collector.ClosestHit;
* var hitDistance = raycastLength * hit.Fraction;
*
* Debug.LogWarning ( "Hit dist: " + hitDistance ) ;
*
*
* }).Schedule () ;
*/
// Debug.Log ( "runs" ) ;
// EntityCommandBuffer.ParallelWriter ecbp = becb.CreateCommandBuffer ().AsParallelWriter () ;
// CollisionWorld collisionWorld = buildPhysicsWorldSystem.PhysicsWorld.CollisionWorld ;
// Debug.DrawLine ( pointerRay.origin, pointerRay.direction * 200, Color.blue ) ;
// RaycastInput raycastInput = new RaycastInput () { Start = pointerRay.origin, End = pointerRay.direction * 200, Filter = CollisionFilter.Default } ;
// UnityEngine.Ray pointerRay = Camera.main.ScreenPointToRay ( Input.mousePosition ) ;
Vector3 V3_point = Camera.main.ScreenToWorldPoint(Input.mousePosition);
Debug.DrawLine(V3_point, V3_point - Vector3.up * 200, Color.blue);
CollisionFilter collisionFilter = new CollisionFilter()
{
BelongsTo = default, // 1,
static private void InternalTickCallbackNative(IntPtr world, float timeStep)
{
CollisionWorld cw = _native2ManagedMap[world];
((DynamicsWorld)cw)._callback((DynamicsWorld)cw, timeStep);
}
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);
}
///btActionInterface interface
public virtual void UpdateAction(CollisionWorld collisionWorld, float deltaTime)
{
PreStep(collisionWorld);
PlayerStep(collisionWorld, deltaTime);
}
public void StoreIslandActivationState(CollisionWorld world)
{
btSimulationIslandManager_storeIslandActivationState(Native, world.Native);
}
public void UpdateActivationState(CollisionWorld colWorld, Dispatcher dispatcher)
{
btSimulationIslandManager_updateActivationState(Native, colWorld.Native,
dispatcher.Native);
}
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);
}
public void cast(CollisionWorld cw)
{
#if USE_BT_CLOCK
frame_timer.reset();
#endif //USE_BT_CLOCK
#if BATCH_RAYCASTER
if (gBatchRaycaster == null)
{
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& outResult = (*gBatchRaycaster)[i];
hit[i].setInterpolate3(source[i], dest[i], outResult.hitFraction);
normal[i] = outResult.hitNormal;
normal[i] = normal[i].Normalize();
}
#else
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
using (ClosestRayResultCallback cb = BulletGlobals.ClosestRayResultCallbackPool.Get())
{
cb.Initialize(source[i], dest[i]);
cw.RayTest(ref source[i], ref dest[i], cb);
if (cb.HasHit())
{
hit[i] = cb.m_hitPointWorld;
normal[i] = cb.m_hitNormalWorld;
normal[i] = IndexedVector3.Normalize(normal[i]);
}
else
{
hit[i] = dest[i];
normal[i] = new IndexedVector3(1.0f, 0.0f, 0.0f);
}
}
}
#if USE_BT_CLOCK
ms += frame_timer.getTimeMilliseconds();
#endif //USE_BT_CLOCK
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;
//printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
#endif
}
/// <summary>
/// Gets the final position of a character attempting to move from a starting
/// location with a given movement. The goal of this is to move the character
/// but have the character 'bounce' off of objects at angles that are
/// are along the plane perpendicular to the normal of the surface hit.
/// This will cancel motion through the object and instead deflect it
/// into another direction giving the effect of sliding along objects
/// while the character's momentum is absorbed into the wall.
/// </summary>
/// <param name="commandBuffer">Command buffer for adding push events to objects</param>
/// <param name="jobIndex">Index of this job for command buffer use</param>
/// <param name="collisionWorld">World for checking collisions and other colliable objects</param>
/// <param name="start">Starting location</param>
/// <param name="movement">Intended direction of movement</param>
/// <param name="collider">Collider controlling the character</param>
/// <param name="entityIndex">Index of this entity</param>
/// <param name="rotation">Current character rotation</param>
/// <param name="physicsMassAccessor">Accessor to physics mass components</param>
/// <param name="anglePower">Power to raise decay of movement due to
/// changes in angle between intended movement and angle of surface.
/// Will be angleFactor= 1 / (1 + normAngle) where normAngle is a normalized value
/// between 0-1 where 1 is max angle (90 deg) and 0 is min angle (0 degrees).
/// AnglePower is the power to which the angle factor is raised
/// for a sharper decline. Value of zero negates this property.
/// <param name="maxBounces">Maximum number of bounces when moving.
/// After this has been exceeded the bouncing will stop. By default
/// this is one assuming that each move is fairly small this should approximate
/// normal movement.</param>
/// <param name="pushPower">Multiplier for power when pushing on an object.
/// Larger values mean more pushing.</param>
/// <param name="pushDecay">How much does the current push decay the remaining
/// movement by. Values between [0, 1]. A zero would mean all energy is lost
/// when pushing, 1 means no momentum is lost by pushing. A value of 0.5
/// would mean half of the energy is lost</param>
/// <param name="epsilon">Epsilon parameter for pushing away from objects to avoid colliding
/// with static objects. Should be some small float value that can be
/// tuned for how much to push away while not allowing being shoved into objects.</param>
/// <returns>The final location of the character.</returns>
public static unsafe float3 ProjectValidMovement(
EntityCommandBuffer.ParallelWriter commandBuffer,
int jobIndex,
CollisionWorld collisionWorld,
float3 start,
float3 movement,
PhysicsCollider collider,
int entityIndex,
quaternion rotation,
ComponentDataFromEntity <PhysicsMass> physicsMassGetter,
float anglePower = 2,
int maxBounces = 1,
float pushPower = 25,
float pushDecay = 0,
float epsilon = 0.001f)
{
float3 from = start; // Starting location of movement
float3 remaining = movement; // Remaining momentum
int bounces = 0; // current number of bounces
// Continue computing while there is momentum and bounces remaining
while (math.length(remaining) > epsilon && bounces <= maxBounces)
{
// Get the target location given the momentum
float3 target = from + remaining;
// Do a cast of the collider to see if an object is hit during this
// movement action
var input = new ColliderCastInput()
{
Start = from,
End = target,
Collider = collider.ColliderPtr,
Orientation = rotation
};
SelfFilteringClosestHitCollector <ColliderCastHit> hitCollector =
new SelfFilteringClosestHitCollector <ColliderCastHit>(entityIndex, 1.0f, collisionWorld);
bool collisionOcurred = collisionWorld.CastCollider(input, ref hitCollector);
if (!collisionOcurred && hitCollector.NumHits == 0)
{
// If there is no hit, target can be returned as final position
return(target);
}
Unity.Physics.ColliderCastHit hit = hitCollector.ClosestHit;
// Set the fraction of remaining movement (minus some small value)
from = from + remaining * hit.Fraction;
// Push slightly along normal to stop from getting caught in walls
from = from + hit.SurfaceNormal * epsilon;
// Apply some force to the object hit if it is moveable, Apply force on entity hit
bool isKinematic = physicsMassGetter.HasComponent(hit.Entity) && IsKinematic(physicsMassGetter[hit.Entity]);
if (hit.RigidBodyIndex < collisionWorld.NumDynamicBodies && !isKinematic)
{
commandBuffer.AddBuffer <PushForce>(jobIndex, hit.Entity);
commandBuffer.AppendToBuffer(jobIndex, hit.Entity, new PushForce()
{
force = movement * pushPower, point = hit.Position
});
// If pushing something, reduce remaining force significantly
remaining *= pushDecay;
}
// Get angle between surface normal and remaining movement
float angleBetween = math.length(math.dot(hit.SurfaceNormal, remaining)) / math.length(remaining);
// Normalize angle between to be between 0 and 1
angleBetween = math.min(KCCUtils.MaxAngleShoveRadians, math.abs(angleBetween));
float normalizedAngle = angleBetween / KCCUtils.MaxAngleShoveRadians;
// Create angle factor using 1 / (1 + normalizedAngle)
float angleFactor = 1.0f / (1.0f + normalizedAngle);
// If the character hit something
// Reduce the momentum by the remaining movement that ocurred
remaining *= (1 - hit.Fraction) * math.pow(angleFactor, anglePower);
// Rotate the remaining remaining movement to be projected along the plane
// of the surface hit (emulate pushing against the object)
// A is our vector and B is normal of plane
// A || B = B × (A×B / |B|) / |B|
// From http://www.euclideanspace.com/maths/geometry/elements/plane/lineOnPlane/index.htm
float3 planeNormal = hit.SurfaceNormal;
float momentumLeft = math.length(remaining);
remaining = math.cross(planeNormal, math.cross(remaining, planeNormal) / math.length(planeNormal)) / math.length(planeNormal);
remaining = math.normalizesafe(remaining) * momentumLeft;
// Track number of times the character has bounced
bounces++;
}
return(from);
}
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;
}
collisionPair.GetAllContactManifolds(m_manifoldArray);
for (int j = 0; j < m_manifoldArray.Count; j++)
{
var manifoldId = AlignedManifoldArray.btAlignedManifoldArray_at(m_manifoldArray._native, j);
var bodyId = PersistentManifold.btPersistentManifold_getBody0(manifoldId);
var numContacts = PersistentManifold.btPersistentManifold_getNumContacts(manifoldId);
float directionSign = bodyId == m_ghostObject._native ? -1f : 1f;
for (int p = 0; p < numContacts; p++)
{
var manifoldPointId = PersistentManifold.btPersistentManifold_getContactPoint(manifoldId, p);
float dist = ManifoldPoint.btManifoldPoint_getDistance(manifoldPointId);
if (dist < 0.0f)
{
Vector3 normalWorldOnB;
ManifoldPoint.btManifoldPoint_getNormalWorldOnB(manifoldPointId, out normalWorldOnB);
if (dist < maxPen)
{
maxPen = dist;
}
var counterPenDir = normalWorldOnB * directionSign;;
m_currentPosition += counterPenDir * dist;
penetration = true;
if (counterPenDir.Dot(Vector3.UnitY) > 0)
{
m_verticalVelocity = 0;
}
}
}
}
}
Matrix newTrans = m_ghostObject.WorldTransform;
newTrans.Origin = m_currentPosition;
m_ghostObject.WorldTransform = newTrans;
return(penetration);
}
public void Cast(CollisionWorld cw, float frameDelta)
{
#if BATCH_RAYCASTER
if (!batchRaycaster)
{
return;
}
batchRaycaster.ClearRays();
foreach (var ray in _rays)
{
batchRaycaster.AddRay(ray.Source, ray.Destination);
}
batchRaycaster.PerformBatchRaycast();
for (int i = 0; i < batchRaycaster.NumRays; i++)
{
const SpuRaycastTaskWorkUnitOut& out = (*batchRaycaster)[i];
_rays[i].HitPoint.SetInterpolate3(_source[i], _destination[i], out.HitFraction);
_rays[i].Normal = out.hitNormal;
_rays[i].Normal.Normalize();
}
#else
foreach (var ray in _rays)
{
using (var cb = new ClosestRayResultCallback(ref ray.Source, ref ray.Destination))
{
cw.RayTestRef(ref ray.Source, ref ray.Destination, cb);
if (cb.HasHit)
{
ray.HitPoint = cb.HitPointWorld;
ray.Normal = cb.HitNormalWorld;
ray.Normal.Normalize();
}
else
{
ray.HitPoint = ray.Destination;
ray.Normal = 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;
PrintStats();
_time = 0;
_frameCount = 0;
}
#endif
}
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;
}
}
/// <summary>
/// Processes a specific entity in the chunk.
/// </summary>
/// <param name="entity"></param>
/// <param name="controller"></param>
/// <param name="position"></param>
/// <param name="rotation"></param>
/// <param name="collider"></param>
/// <param name="collisionWorld"></param>
private void HandleChunk(ref Entity entity, ref CharacterControllerComponent controller, ref Translation position, ref Rotation rotation, ref PhysicsCollider collider, ref CollisionWorld collisionWorld)
{
float3 epsilon = new float3(0.0f, Epsilon, 0.0f) * -math.normalize(controller.Gravity);
float3 currPos = position.Value + epsilon;
quaternion currRot = rotation.Value;
float3 gravityVelocity = controller.Gravity * DeltaTime;
float3 verticalVelocity = (controller.VerticalVelocity + gravityVelocity);
float3 horizontalVelocity = (controller.CurrentDirection * controller.CurrentMagnitude * controller.Speed * DeltaTime);
if (controller.IsGrounded)
{
if (controller.Jump)
{
verticalVelocity = controller.JumpStrength * -math.normalize(controller.Gravity);
}
else
{
float3 gravityDir = math.normalize(gravityVelocity);
float3 verticalDir = math.normalize(verticalVelocity);
if (MathUtils.FloatEquals(math.dot(gravityDir, verticalDir), 1.0f))
{
verticalVelocity = new float3();
}
}
}
HandleHorizontalMovement(ref horizontalVelocity, ref entity, ref currPos, ref currRot, ref controller, ref collider, ref collisionWorld);
currPos += horizontalVelocity;
HandleVerticalMovement(ref verticalVelocity, ref entity, ref currPos, ref currRot, ref controller, ref collider, ref collisionWorld);
currPos += verticalVelocity;
CorrectForCollision(ref entity, ref currPos, ref currRot, ref controller, ref collider, ref collisionWorld);
DetermineIfGrounded(entity, ref currPos, ref epsilon, ref controller, ref collider, ref collisionWorld);
position.Value = currPos - epsilon;
}
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>
/// Performs a collision correction at the specified position.
/// </summary>
/// <param name="entity"></param>
/// <param name="currPos"></param>
/// <param name="currRot"></param>
/// <param name="controller"></param>
/// <param name="collider"></param>
/// <param name="collisionWorld"></param>
private void CorrectForCollision(ref Entity entity, ref float3 currPos, ref quaternion currRot, ref CharacterControllerComponent controller, ref PhysicsCollider collider, ref CollisionWorld collisionWorld)
{
RigidTransform transform = new RigidTransform()
{
pos = currPos,
rot = currRot
};
// Use a subset sphere within our collider to test against.
// We do not use the collider itself as some intersection (such as on ramps) is ok.
var offset = -math.normalize(controller.Gravity) * 0.1f;
var sampleCollider = new PhysicsCollider()
{
Value = SphereCollider.Create(new SphereGeometry()
{
Center = currPos + offset,
Radius = 0.1f
})
};
if (PhysicsUtils.ColliderDistance(out DistanceHit smallestHit, sampleCollider, 0.1f, transform, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, null, ColliderData, Allocator.Temp))
{
if (smallestHit.Distance < 0.0f)
{
currPos += math.abs(smallestHit.Distance) * smallestHit.SurfaceNormal;
}
}
}
///btActionInterface interface
public virtual void UpdateAction(CollisionWorld collisionWorld, float step)
{
UpdateVehicle(step);
}
/// <summary>
/// Determines if the character is resting on a surface.
/// </summary>
/// <param name="entity"></param>
/// <param name="currPos"></param>
/// <param name="epsilon"></param>
/// <param name="collider"></param>
/// <param name="collisionWorld"></param>
/// <returns></returns>
private unsafe static void DetermineIfGrounded(Entity entity, ref float3 currPos, ref float3 epsilon, ref CharacterControllerComponent controller, ref PhysicsCollider collider, ref CollisionWorld collisionWorld)
{
var aabb = collider.ColliderPtr->CalculateAabb();
float mod = 0.15f;
float3 samplePos = currPos + new float3(0.0f, aabb.Min.y, 0.0f);
float3 gravity = math.normalize(controller.Gravity);
float3 offset = (gravity * 0.1f);
float3 posLeft = samplePos - new float3(aabb.Extents.x * mod, 0.0f, 0.0f);
float3 posRight = samplePos + new float3(aabb.Extents.x * mod, 0.0f, 0.0f);
float3 posForward = samplePos + new float3(0.0f, 0.0f, aabb.Extents.z * mod);
float3 posBackward = samplePos - new float3(0.0f, 0.0f, aabb.Extents.z * mod);
controller.IsGrounded = PhysicsUtils.Raycast(out RaycastHit centerHit, samplePos, samplePos + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) ||
PhysicsUtils.Raycast(out RaycastHit leftHit, posLeft, posLeft + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) ||
PhysicsUtils.Raycast(out RaycastHit rightHit, posRight, posRight + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) ||
PhysicsUtils.Raycast(out RaycastHit forwardHit, posForward, posForward + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) ||
PhysicsUtils.Raycast(out RaycastHit backwardHit, posBackward, posBackward + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp);
}
private void FixedUpdate()
{
CollisionWorld collisionWorld = GetWorld().World;
collisionWorld.Dispatcher.DispatchAllCollisionPairs(m_ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher);
//m_currentPosition = m_ghostObject.WorldTransform.Origin;
//float maxPen = 0f;
objsCurrentlyInContactWith.Clear();
for (int i = 0; i < m_ghostObject.OverlappingPairCache.NumOverlappingPairs; i++)
{
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(manifoldArray);
}
CollisionObject otherObj = null;
if (manifoldArray.Count > 0)
{
PersistentManifold pm = manifoldArray[0];
if (pm.Body0 == collisionObject)
{
otherObj = pm.Body1;
}
else
{
otherObj = pm.Body0;
}
}
else
{
continue;
}
objsCurrentlyInContactWith.Add(otherObj);
if (!objsIWasInContactWithLastFrame.Contains(otherObj))
{
BOnTriggerEnter(otherObj, manifoldArray);
}
else
{
BOnTriggerStay(otherObj, manifoldArray);
}
}
objsIWasInContactWithLastFrame.ExceptWith(objsCurrentlyInContactWith);
foreach (CollisionObject co in objsIWasInContactWithLastFrame)
{
BOnTriggerExit(co);
}
//swap the hashsets so objsIWasInContactWithLastFrame now contains the list of objs.
HashSet <CollisionObject> temp = objsIWasInContactWithLastFrame;
objsIWasInContactWithLastFrame = objsCurrentlyInContactWith;
objsCurrentlyInContactWith = temp;
}
protected override void OnUpdate()
{
CollisionFilter collisionFilter = new CollisionFilter()
{
BelongsTo = ~0u,
CollidesWith = ~0u, // all 1s, so all layers, collide with everything
GroupIndex = 0
};
EntityManager entityManager = World.EntityManager;
CollisionWorld collisionWorld = physicsWorld.PhysicsWorld.CollisionWorld;
Dependency = Entities.WithAll <AnimalTag>().ForEach((ref AnimalMovementData mvmtData, in Translation translation) => {
float3 start = new float3(translation.Value.x, translation.Value.y, translation.Value.z + 0.2f);
#region front
RaycastInput inputFront = new RaycastInput()
{
Start = start,
End = new float3(translation.Value.x, translation.Value.y, translation.Value.z + (2.5f * mvmtData.movementSpeed)),
Filter = collisionFilter
};
RaycastHit hitFront = new RaycastHit();
bool hasHitFront = collisionWorld.CastRay(inputFront, out hitFront);
#endregion
#region front right
RaycastInput inputFR = new RaycastInput()
{
Start = start,
End = new float3(translation.Value.x + (2.5f * mvmtData.movementSpeed), translation.Value.y, translation.Value.z + (2.5f * mvmtData.movementSpeed)),
Filter = collisionFilter
};
RaycastHit hitFR = new RaycastHit();
bool hasHitFR = collisionWorld.CastRay(inputFR, out hitFR);
#endregion
#region front left
RaycastInput inputFL = new RaycastInput()
{
Start = start,
End = new float3(translation.Value.x - (2.5f * mvmtData.movementSpeed), translation.Value.y, translation.Value.z + (2.5f * mvmtData.movementSpeed)),
Filter = collisionFilter
};
RaycastHit hitFL = new RaycastHit();
bool hasHitFL = collisionWorld.CastRay(inputFR, out hitFL);
#endregion
if (hasHitFront)
{
float3 normal = hitFront.SurfaceNormal;
//AnimalMovementData mvmtDataHitEntity = entityManager.GetComponentData<AnimalMovementData>(hitEntity);
//float maxSpeed = math.max(mvmtData.movementSpeed, mvmtDataHitEntity.movementSpeed);
float dotProduct = math.dot(math.normalize(mvmtData.targetDirection - translation.Value), math.normalize(hitFront.SurfaceNormal));
float angle = 0.262f;
angle += 1f - math.abs(dotProduct);
//angle += (1f - (maxSpeed * 0.1f));
angle = math.clamp(angle, 0f, StaticValues.AVOIDANCE_MIN_ANGLE);
quaternion newRotation = quaternion.RotateY(angle);
float3 newDirection = math.rotate(newRotation, mvmtData.direction);
newDirection = math.normalizesafe(newDirection);
// Set EntityA's target direction
mvmtData.targetDirection = newDirection;
}
else if (hasHitFR)
{
float3 normal = hitFR.SurfaceNormal;
//AnimalMovementData mvmtDataHitEntity = entityManager.GetComponentData<AnimalMovementData>(hitEntity);
//float maxSpeed = math.max(mvmtData.movementSpeed, mvmtDataHitEntity.movementSpeed);
float dotProduct = math.dot(math.normalize(mvmtData.targetDirection - translation.Value), math.normalize(hitFR.SurfaceNormal));
float angle = 0.262f;
angle += 1f - math.abs(dotProduct);
//angle += (1f - (maxSpeed * 0.1f));
angle = math.clamp(angle, 0f, StaticValues.AVOIDANCE_MIN_ANGLE);
quaternion newRotation = quaternion.RotateY(angle);
float3 newDirection = math.rotate(newRotation, mvmtData.direction);
newDirection = math.normalizesafe(newDirection);
// Set EntityA's target direction
mvmtData.targetDirection = newDirection;
}
else if (hasHitFL)
{
float3 normal = hitFR.SurfaceNormal;
//AnimalMovementData mvmtDataHitEntity = entityManager.GetComponentData<AnimalMovementData>(hitEntity);
//float maxSpeed = math.max(mvmtData.movementSpeed, mvmtDataHitEntity.movementSpeed);
float dotProduct = math.dot(math.normalize(mvmtData.targetDirection - translation.Value), math.normalize(hitFR.SurfaceNormal));
float angle = 0.262f;
angle += 1f - math.abs(dotProduct);
//angle += (1f - (maxSpeed * 0.1f));
angle = math.clamp(angle, 0f, StaticValues.AVOIDANCE_MIN_ANGLE);
quaternion newRotation = quaternion.RotateY(angle);
float3 newDirection = math.rotate(newRotation, mvmtData.direction);
newDirection = math.normalizesafe(newDirection);
// Set EntityA's target direction
mvmtData.targetDirection = newDirection;
}
}).Schedule(Dependency);
Dependency.Complete();
}
//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 ConstraintSolver solver,
out SoftBodyWorldInfo softBodyWorldInfo
)
{
bool success = true;
if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf)
{
Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType=SoftBodyRigidBodyCollisionConf. Switching");
m_collisionType = CollisionConfType.SoftBodyRigidBodyCollisionConf;
success = false;
}
collisionConfig = null;
switch (m_collisionType)
{
default:
case CollisionConfType.DefaultDynamicsWorldCollisionConf:
collisionConfig = new DefaultCollisionConfiguration();
break;
case CollisionConfType.SoftBodyRigidBodyCollisionConf:
collisionConfig = new SoftBodyRigidBodyCollisionConfiguration();
break;
}
dispatcher = new CollisionDispatcher(collisionConfig);
switch (m_broadphaseType)
{
default:
case BroadphaseType.DynamicAABBBroadphase:
broadphase = new DbvtBroadphase();
break;
case BroadphaseType.Axis3SweepBroadphase:
broadphase = new AxisSweep3(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies);
break;
case BroadphaseType.Axis3SweepBroadphase_32bit:
broadphase = new AxisSweep3_32Bit(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies);
break;
case BroadphaseType.SimpleBroadphase:
broadphase = null;
success = false;
break;
}
world = null;
softBodyWorldInfo = null;
solver = null;
switch (m_worldType)
{
case WorldType.CollisionOnly:
world = new CollisionWorld(dispatcher, broadphase, collisionConfig);
break;
default:
case WorldType.RigidBodyDynamics:
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConfig);
break;
case WorldType.MultiBodyWorld:
MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver();
constraintSolver = mbConstraintSolver;
world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig);
break;
case WorldType.SoftBodyAndRigidBody:
SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver();
constraintSolver = siConstraintSolver;
siConstraintSolver.RandSeed = sequentialImpulseConstraintSolverRandomSeed;
world = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, siConstraintSolver, CollisionConf);
world.DispatchInfo.EnableSpu = true;
SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)world;
_sworld.WorldInfo.SparseSdf.Initialize();
_sworld.WorldInfo.SparseSdf.Reset();
_sworld.WorldInfo.AirDensity = 1.2f;
_sworld.WorldInfo.WaterDensity = 0;
_sworld.WorldInfo.WaterOffset = 0;
_sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero;
_sworld.WorldInfo.Gravity = m_gravity.ToBullet();
break;
}
if (world is DiscreteDynamicsWorld)
{
((DiscreteDynamicsWorld)world).Gravity = m_gravity.ToBullet();
}
if (_doDebugDraw)
{
DebugDrawUnity db = new DebugDrawUnity();
db.DebugMode = _debugDrawMode;
world.DebugDrawer = db;
}
return(success);
}
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
{
Matrix start = Matrix.Identity, end = Matrix.Identity;
m_targetPosition = m_currentPosition + walkMove;
float fraction = 1.0f;
float distance2 = (m_currentPosition - m_targetPosition).LengthSquared;
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0)
{
start.Origin = (m_currentPosition);
end.Origin = (m_targetPosition);
Vector3 sweepDirNegative = m_currentPosition - m_targetPosition;
_callback.MinSlopeDot = 0;
_callback.Up = sweepDirNegative;
_callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
_callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
_callback.ClosestHitFraction = 1;
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)
{
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." */
Vector3.Dot(ref currentDir, ref m_normalizedDirection, out var dot);
if (dot <= 0.0f)
{
break;
}
}
else
{
break;
}
}
else
{
m_currentPosition = m_targetPosition;
}
}
}
protected void Dispose(bool disposing)
{
if (debugType >= BDebug.DebugType.Debug)
{
Debug.Log("BDynamicsWorld Disposing physics.");
}
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 StepDown(CollisionWorld collisionWorld, float dt)
{
Matrix start, end, end_double;
bool runonce = false;
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 = Vector3.UnitY * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
_callback.MinSlopeDot = m_maxSlopeCosine;
_callback.Up = Vector3.UnitY;
_callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
_callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
bool hasFirstHit;
float hitFraction;
Vector3 pointWorld;
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);
bool has_hit_first = false;
bool has_hit_second = false;
if (m_useGhostObjectSweepTest)
{
_callback.ClosestHitFraction = 1;
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
pointWorld = _callback.HitPointWorld;
has_hit_first = _callback.HasHit;
hitFraction = _callback.ClosestHitFraction;
if (!has_hit_first)
{
_callback.ClosestHitFraction = 1;
//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, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
has_hit_second = _callback.HasHit;
}
}
else
{
_callback.ClosestHitFraction = 1;
// this works....
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
pointWorld = _callback.HitPointWorld;
has_hit_first = _callback.HasHit;
hitFraction = _callback.ClosestHitFraction;
if (!has_hit_first)
{
_callback.ClosestHitFraction = 1;
//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, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
has_hit_second = _callback.HasHit;
}
}
hasFirstHit = has_hit_first;
float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
bool has_hit;
if (bounce_fix == true)
{
has_hit = has_hit_first || has_hit_second;
}
else
{
has_hit = has_hit_second;
}
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 = Vector3.UnitY * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop2;
runonce = true;
continue; //re-run previous tests
}
break;
}
if (hasFirstHit || runonce)
{
// we dropped a fraction of the height -> hit floor
float fraction = (m_currentPosition.Y - pointWorld.Y) / 2;
if (bounce_fix)
{
if (full_drop)
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, hitFraction, 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, hitFraction, 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 = Vector3.UnitY * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
m_currentPosition = m_targetPosition;
}
}
/// <summary>
/// Creates a self filtering object collision detection
/// </summary>
/// <param name="ignore">index of entity to ignore hitting</param>
/// <param name="allow">index of entity to allow to hit, leave empty to allow everything (except the ignroe)</param>
/// <param name="maxFraction">Maximum fraction that an object can be encountered
/// as a portion of the current raycast draw</param>
/// <param name="collisionWorld">World of all colliable objects</param>
public FilteringClosestHitCollector(int ignore, int allow, float maxFraction, CollisionWorld collisionWorld)
{
this.MaxFraction = maxFraction;
this.ClosestHit = default;
this.NumHits = 0;
this.ignore = ignore;
this.allow = allow;
this.collisionWorld = collisionWorld;
}
protected unsafe override void OnUpdate()
{
var _ecb = m_EntityCommandBufferSystem.CreateCommandBuffer();
var _ecbo = m_EntityCommandBufferSystemo.CreateCommandBuffer();
var _ecbEndSimilation = m_endSimulationEntityCommandBufferSystem.CreateCommandBuffer();
var _ecbBeginPresentation = m_beginPresentationEntityCommandBufferSystem.CreateCommandBuffer();
var _physicsWorldSystem = World.DefaultGameObjectInjectionWorld.GetExistingSystem <Unity.Physics.Systems.BuildPhysicsWorld>();
CollisionWorld _collisionWorld = _physicsWorldSystem.PhysicsWorld.CollisionWorld;
var _physicsWorld = _physicsWorldSystem.PhysicsWorld;
float3 _startPosition = ECS_RTSControls.instance._startScreenPosition;
float3 _endPosition = ECS_RTSControls.instance._endScreenPosition;
float3 _lowerLeftPosition = ECS_RTSControls.instance._lowerLeftScreenPosition;
float3 _upperRightPosition = ECS_RTSControls.instance._upperRightScreenPosition;
Dependency = JobHandle.CombineDependencies(Dependency, m_EndFramePhysicsSystem.GetOutputDependency());
if (_inputReady)
{
if (Input_Mouse_0_Up == true && Input_Mouse_0_Up != EntityManager.HasComponent <Input_Mouse_0_Up>(_inputEntity))
{
Input_Mouse_0_GetDown = true;
Input_Mouse_0_Up = false;
}
if (Input_Mouse_0_Up == false && (Input_Mouse_0_Up != EntityManager.HasComponent <Input_Mouse_0_Up>(_inputEntity)))
{
Input_Mouse_0_GetUp = true;
Input_Mouse_0_Up = true;
}
if (Input_Mouse_1_Up == true && (Input_Mouse_1_Up != EntityManager.HasComponent <Input_Mouse_1_Up>(_inputEntity)))
{
Input_Mouse_1_GetDown = true;
Input_Mouse_1_Up = false;
}
if (Input_Mouse_1_Up == false && (Input_Mouse_1_Up != EntityManager.HasComponent <Input_Mouse_1_Up>(_inputEntity)))
{
Input_Mouse_1_GetUp = true;
Input_Mouse_1_Up = true;
}
}
else
{
if (_queryInput.CalculateEntityCount() > 0)
{
_inputEntity = _queryInput.GetSingletonEntity();
_inputReady = true;
Input_Mouse_0_Up = EntityManager.HasComponent <Input_Mouse_0_Up>(_inputEntity);
Input_Mouse_1_Up = EntityManager.HasComponent <Input_Mouse_1_Up>(_inputEntity);
}
}
if (Time.ElapsedTime > 4)
{
/*this.Dependency = Entities.WithoutBurst().ForEach((in Translation _translation, in UnitSelected _unitSelected, in Selectable _selectable, in GrisTag _grisTag, in Entity _entity) =>
* {
* var e = _ecbEndSimilation.Instantiate(PrefabConvert._DalleVerte);
* _ecbEndSimilation.SetComponent<Translation>(e, _translation);
* _ecbEndSimilation.DestroyEntity(_entity);
* Debug.Log("Unity3d : 'va te faire foutre'");
*
* }).Schedule(this.Dependency);*/
}
if (Input_Mouse_0_GetUp == true)
{
bool selectOnlyOneEntity = false;
float selectionAreaMinSize = 10f;
float selectionAreaSize = math.distance(_lowerLeftPosition, _upperRightPosition);
if (selectionAreaSize < selectionAreaMinSize)
{
// Selection area too small
_lowerLeftPosition += new float3(-1, -1, 0) * (selectionAreaMinSize - selectionAreaSize) * .5f;
_upperRightPosition += new float3(+1, +1, 0) * (selectionAreaMinSize - selectionAreaSize) * .5f;
selectOnlyOneEntity = true;
}
// Deselect all selected Entities
this.Dependency = Entities.WithAll <UnitSelected>().ForEach((Entity entity) => {
_ecb.RemoveComponent <UnitSelected>(entity);
}).Schedule(this.Dependency);
Unity.Physics.RaycastHit _hit1 = new Unity.Physics.RaycastHit();
Unity.Physics.RaycastHit _hit2 = new Unity.Physics.RaycastHit();
Unity.Physics.RaycastHit _hit3 = new Unity.Physics.RaycastHit();
Unity.Physics.RaycastHit _hit4 = new Unity.Physics.RaycastHit();
Unity.Physics.RaycastHit _hit5 = new Unity.Physics.RaycastHit();
var _ray1 = ECS_RTSControls.instance._ray1;
var _ray2 = ECS_RTSControls.instance._ray2;
var _ray3 = ECS_RTSControls.instance._ray3;
var _ray4 = ECS_RTSControls.instance._ray4;
var _ray5 = ECS_RTSControls.instance._ray5;
/*RaycastInput _inputo = new RaycastInput()
* {
* Start = _ray1.origin,
* End = _ray1.origin + _ray1.direction * 1000,
* Filter = new CollisionFilter()
* {
* BelongsTo = (uint)10 << 10,
* CollidesWith = (uint)7 << 8,
* GroupIndex = 8
* }
* };*/
GetRaycastHit(_collisionWorld, _ray1, _ray2, _ray3, _ray4, _ray5, out _hit1, out _hit2, out _hit3, out _hit4, out _hit5);
var _filter = new CollisionFilter()
{
BelongsTo = ~1u,
CollidesWith = ~1u, // all 1s, so all layers, collide with everything
GroupIndex = 0
};
NativeList <ColliderCastHit> _allHits = new NativeList <ColliderCastHit>(Allocator.TempJob);
_polygonCollider = Unity.Physics.PolygonCollider.CreateQuad(_hit1.Position, _hit2.Position, _hit3.Position, _hit4.Position, _filter);
/*SphereGeometry sphereGeometry = new SphereGeometry() { Center = float3.zero, Radius = 3 };
* _sphereCollider = Unity.Physics.SphereCollider.Create(sphereGeometry, _filter);*/
/*
* Unity.Physics.ColliderCastInput colliderCastInput = new ColliderCastInput()
* {
* Collider = (Unity.Physics.Collider*)_sphereCollider.GetUnsafePtr(),
* Orientation = ECS_RTSControls.instance._camera.transform.rotation,
* Start = ECS_RTSControls.instance._camera.transform.position,
* End = _ray1.origin + _ray1.direction * 1000
* };*/
var _moyPoly = new float3((_hit1.Position.x + _hit2.Position.x + _hit3.Position.x + _hit4.Position.x) / 4,
(_hit1.Position.y + _hit2.Position.y + _hit3.Position.y + _hit4.Position.y) / 4,
(_hit1.Position.z + _hit2.Position.z + _hit3.Position.z + _hit4.Position.z) / 4);
Debug.Log(_moyPoly + "_moyPoly");
//var _pouitCollider = BlobAssetReference<Unity.Physics.Collider>.Create(_polygonCollider.GetUnsafePtr(), 1);
/* this.Dependency = Job.WithoutBurst().WithCode(() =>
* {
* var e = _ecb.Instantiate(PrefabConvert._CubeRouge);
* _ecb.SetComponent<Translation>(e, new Translation { Value = _hit1.Position + new float3(0, 5, 0) });
* e = _ecb.Instantiate(PrefabConvert._CubeRouge);
* _ecb.SetComponent<Translation>(e, new Translation { Value = _hit2.Position + new float3(0, 5, 0) });
* e = _ecb.Instantiate(PrefabConvert._CubeRouge);
* _ecb.SetComponent<Translation>(e, new Translation { Value = _hit3.Position + new float3(0, 5, 0) });
* e = _ecb.Instantiate(PrefabConvert._CubeRouge);
* _ecb.SetComponent<Translation>(e, new Translation { Value = _hit4.Position + new float3(0, 5, 0) });
* }).Schedule(this.Dependency);
* Debug.Log(_dalleSelectedQuery.CalculateEntityCount() + "_dalleSelectedQueryCount");
* Debug.Log(_dalleQuery.CalculateEntityCount() + "_dalleQueryCount");
*
* /*Mesh _mesh = new Mesh { }
*
* RenderMesh huiaef = new RenderMesh { mesh = }
*
* this.Dependency.Complete();*/
ColliderCastJob _colliderCastJob = new ColliderCastJob();
_colliderCastJob.CollectAllHits = true;
_colliderCastJob.Collider = (Unity.Physics.Collider *)_polygonCollider.GetUnsafePtr();
_colliderCastJob.ColliderCastHits = _allHits;
_colliderCastJob.End = new float3(0, -2, 0); // _moyPoly;
_colliderCastJob.Orientation = quaternion.identity; //ECS_RTSControls.instance._camera.transform.rotation;
_colliderCastJob.Start = /*_moyPoly +*/ new float3(0, 5, 0);
_colliderCastJob.World = _physicsWorld;
Dependency = _colliderCastJob.ScheduleSingle(_query, this.Dependency);
//World.GetOrCreateSystem<StepPhysicsWorld>().FinalSimulationJobHandle.Complete();
this.Dependency.Complete();
/*Debug.Log(_query.CalculateEntityCount() + "_queryMovestats");
* Debug.Log(_allHits.Length + "_allHits.Length");*/
this.Dependency = Job.WithoutBurst().WithDisposeOnCompletion(_allHits).WithCode(() =>
{
for (int i = 0; i < _allHits.Length; i++)
{
_ecbEndSimilation.AddComponent(_allHits[i].Entity, new UnitSelected());
}
Debug.Log(_allHits.Length + "entityHits");
}).Schedule(this.Dependency);
this.Dependency.Complete();
}
if (Input_Mouse_1_Up == false)
{
if (Time.ElapsedTime > 2)
{
Debug.Log("yoloZoulou");
this.Dependency = Entities.WithoutBurst().WithAll <PlaneLayer>().WithAll <Player1>().ForEach((Entity entity) => {
_ecb.DestroyEntity(entity);//.RemoveComponent<UnitSelected>(entity);
}).Schedule(this.Dependency);
}
//Debug.Log(Input.GetMouseButtonDown(1));
// Right mouse button down
float3 targetPosition = UtilsClass.GetMouseWorldPosition();
List <float3> movePositionList = GetPositionListAround(targetPosition, new float[] { 10f, 20f, 30f }, new int[] { 5, 10, 20 });
int positionIndex = 0;
Entities.WithAll <UnitSelected>().ForEach((Entity entity, ref MoveStats _moveStats) => {
/*moveTo.position = movePositionList[positionIndex];
* positionIndex = (positionIndex + 1) % movePositionList.Count;
* moveTo.move = true;*/
}).Schedule();
}
//m_EntityCommandBufferSystem.AddJobHandleForProducer(this.Dependency);
World.GetOrCreateSystem <StepPhysicsWorld>().FinalSimulationJobHandle.Complete();
this.Dependency.Complete();
m_EntityCommandBufferSystemo.AddJobHandleForProducer(this.Dependency);
m_endSimulationEntityCommandBufferSystem.AddJobHandleForProducer(this.Dependency);
m_EntityCommandBufferSystemo.AddJobHandleForProducer(this.Dependency);
if (_polygonCollider.IsCreated)
{
_polygonCollider.Dispose();
}
if (_sphereCollider.IsCreated)
{
_sphereCollider.Dispose();
}
Input_Mouse_0_GetUp = false;
Input_Mouse_0_GetDown = false;
}
