public override void ActivateBatch(object world, ulong clusterObjectID)
{
System.Diagnostics.Debug.Assert(m_world == null, "Cannot activate already active object!");
m_world = (HkWorld)world;
ClusterObjectID = clusterObjectID;
IsInWorld = true;
Matrix rigidBodyMatrix = GetRigidBodyTransform();
if (RigidBody != null)
{
RigidBody.SetWorldMatrix(rigidBodyMatrix);
m_world.AddRigidBodyBatch(RigidBody);
}
if (RigidBody2 != null)
{
RigidBody2.SetWorldMatrix(rigidBodyMatrix);
m_world.AddRigidBodyBatch(RigidBody2);
}
if (CharacterProxy != null)
{
CharacterProxy.SetRigidBodyTransform(rigidBodyMatrix);
CharacterProxy.Activate(m_world);
}
foreach (var constraint in m_constraints)
{
m_constraintsAddBatch.Add(constraint);
}
}
public void UpdatePositionAndOrientation()
{
MyCharacter character = Entity as MyCharacter;
if (character == null || character.Physics == null || !character.Physics.IsInWorld)
{
return;
}
if (!IsInWorld && character.Physics.IsInWorld)
{
Enabled = true;
Activate();
}
if (IsInWorld)
{
MatrixD headWorldMatrix = character.GetHeadMatrix(false, forceHeadBone: true);
Matrix rigidBodyMatrix = GetRigidBodyMatrix(headWorldMatrix);
if (RigidBody != null)
{
RigidBody.SetWorldMatrix(rigidBodyMatrix);
}
if (RigidBody2 != null)
{
RigidBody2.SetWorldMatrix(rigidBodyMatrix);
}
}
}
internal void SetRigidBodyTransform(MatrixD worldMatrix)
{
Matrix rigidBodyMatrix = GetRigidBodyMatrix(worldMatrix);
if (RigidBody != null)
{
RigidBody.SetWorldMatrix(rigidBodyMatrix);
}
Matrix m = RigidBody.GetRigidBodyMatrix();
Matrix wM = GetWorldMatrix();
if (RigidBody2 != null)
{
RigidBody2.SetWorldMatrix(rigidBodyMatrix);
}
if (CharacterProxy != null)
{
CharacterProxy.Position = rigidBodyMatrix.Translation;
CharacterProxy.Forward = rigidBodyMatrix.Forward;
CharacterProxy.Up = rigidBodyMatrix.Up;
CharacterProxy.Speed = 0;
if (CharacterProxy.ImmediateSetWorldTransform)
{
CharacterProxy.SetRigidBodyTransform(rigidBodyMatrix);
}
}
}
private void CheckAndDiscardShapes()
{
m_lastDiscardCheck++;
if (m_lastDiscardCheck > SHAPE_DISCARD_CHECK_INTERVAL)
{
m_lastDiscardCheck = 0;
var voxelShape = (HkUniformGridShape)GetShape();
int hits = voxelShape.GetHitsAndClear();
if (m_nearbyEntities.Count == 0 && RigidBody != null && MyFakes.ENABLE_VOXEL_PHYSICS_SHAPE_DISCARDING && voxelShape.ShapeCount > 0 && hits <= SHAPE_DISCARD_THRESHOLD)
{
// RigidBody.GetShape();
Debug.Assert(voxelShape.Base.IsValid);
voxelShape.DiscardLargeData();
if (RigidBody2 != null)
{
voxelShape = (HkUniformGridShape)RigidBody2.GetShape();
hits = voxelShape.GetHitsAndClear();
if (hits <= SHAPE_DISCARD_THRESHOLD)
{
voxelShape.DiscardLargeData();
}
}
}
}
}
private void UpdateRigidBodyShape()
{
if (!m_needsShapeUpdate)
{
return;
}
m_needsShapeUpdate = false;
if (!m_bodiesInitialized)
{
CreateRigidBodies();
}
ProfilerShort.Begin("MyVoxelPhysicsBody.RigidBody.UpdateShape()");
Debug.Assert(RigidBody != null, "RigidBody in voxel physics is null! This must not happen.");
if (RigidBody != null)
{
RigidBody.UpdateShape();
}
if (RigidBody2 != null)
{
RigidBody2.UpdateShape();
}
ProfilerShort.End();
}
private void RecreateWeldedShape(HkShape thisShape)
{
if (RigidBody == null || RigidBody.IsDisposed)
{
Debug.Fail("Missing rigid body");
MyTrace.Send(TraceWindow.Analytics, "Recreating welded shape without RB", string.Format("{0},{1}", Entity.MarkedForClose, WeldInfo.Children.Count));
return;
}
ProfilerShort.Begin("RecreateWeldedShape");
//me.Tranform.Translation = Entity.PositionComp.LocalAABB.Center;
if (WeldInfo.Children.Count == 0)
{
RigidBody.SetShape(thisShape);
if (RigidBody2 != null)
RigidBody2.SetShape(thisShape);
}
else
{
ProfilerShort.Begin("Create shapes");
//m_tmpElements.Add(WeldInfo.MassElement);
m_tmpShapeList.Add(thisShape);
foreach (var child in WeldInfo.Children)
{
var transformShape = new HkTransformShape(child.WeldedRigidBody.GetShape(), ref child.WeldInfo.Transform);
HkShape.SetUserData(transformShape, child.WeldedRigidBody);
m_tmpShapeList.Add(transformShape);
//m_tmpElements.Add(child.WeldInfo.MassElement);
}
//var list = new HkListShape(m_tmpShapeList.ToArray(), HkReferencePolicy.None);
var list = new HkSmartListShape(0);
foreach (var shape in m_tmpShapeList)
list.AddShape(shape);
RigidBody.SetShape(list);
if (RigidBody2 != null)
RigidBody2.SetShape(list);
list.Base.RemoveReference();
WeldedMarkBreakable();
for (int i = 1; i < m_tmpShapeList.Count; i++)
m_tmpShapeList[i].RemoveReference();
m_tmpShapeList.Clear();
ProfilerShort.End();
ProfilerShort.Begin("CalcMassProps");
UpdateMassProps();
//m_tmpElements.Clear();
ProfilerShort.End();
}
ProfilerShort.End();
}
private void CheckAndDiscardShapesOld()
{
var voxelShape = (HkUniformGridShape)GetShape();
if (m_nearbyEntities.Count == 0 && RigidBody != null && MyFakes.ENABLE_VOXEL_PHYSICS_SHAPE_DISCARDING && voxelShape.ShapeCount > 0)
{
// RigidBody.GetShape();
Debug.Assert(voxelShape.Base.IsValid);
voxelShape.DiscardLargeData();
if (RigidBody2 != null)
{
voxelShape = (HkUniformGridShape)RigidBody2.GetShape();
voxelShape.DiscardLargeData();
}
}
}
public void SwitchToRagdollMode(bool deadMode = true, int firstRagdollSubID = 1)
{
Debug.Assert(Enabled, "Trying to switch to ragdoll mode, but Physics are not enabled");
if (MyFakes.ENABLE_RAGDOLL_DEBUG)
{
Debug.WriteLine("MyPhysicsBody.SwitchToRagdollMode");
MyLog.Default.WriteLine("MyPhysicsBody.SwitchToRagdollMode");
}
if (HavokWorld == null || !Enabled)
{
SwitchToRagdollModeOnActivate = true;
m_ragdollDeadMode = deadMode;
return;
}
if (IsRagdollModeActive)
{
Debug.Fail("Ragdoll mode is already active!");
return;
}
Matrix havokMatrix = Entity.WorldMatrix;
havokMatrix.Translation = WorldToCluster(havokMatrix.Translation);
Debug.Assert(havokMatrix.IsValid() && havokMatrix != Matrix.Zero, "Invalid world matrix!");
if (RagdollSystemGroupCollisionFilterID == 0)
{
RagdollSystemGroupCollisionFilterID = m_world.GetCollisionFilter().GetNewSystemGroup();
}
Ragdoll.SetToKeyframed(); // this will disable the bodies to get the impulse when repositioned
Ragdoll.GenerateRigidBodiesCollisionFilters(deadMode ? MyPhysics.CollisionLayers.CharacterCollisionLayer : MyPhysics.CollisionLayers.RagdollCollisionLayer, RagdollSystemGroupCollisionFilterID, firstRagdollSubID);
Ragdoll.ResetToRigPose();
Ragdoll.SetWorldMatrix(havokMatrix);
if (deadMode)
{
Ragdoll.SetToDynamic();
}
if (deadMode)
{
foreach (HkRigidBody body in Ragdoll.RigidBodies)
{
// set the velocities for the bodies
body.AngularVelocity = AngularVelocity;
body.LinearVelocity = LinearVelocity;
}
}
else
{
Ragdoll.ResetVelocities();
}
if (CharacterProxy != null && deadMode)
{
CharacterProxy.Deactivate(HavokWorld);
CharacterProxy.Dispose();
CharacterProxy = null;
}
if (RigidBody != null && deadMode)
{
RigidBody.Deactivate();
HavokWorld.RemoveRigidBody(RigidBody);
RigidBody.Dispose();
RigidBody = null;
}
if (RigidBody2 != null && deadMode)
{
RigidBody2.Deactivate();
HavokWorld.RemoveRigidBody(RigidBody2);
RigidBody2.Dispose();
RigidBody2 = null;
}
foreach (var body in Ragdoll.RigidBodies)
{
body.UserObject = deadMode ? this : null;
// TODO: THIS SHOULD BE SET IN THE RAGDOLL MODEL AND NOT DEFINING IT FOR EVERY MODEL HERE
body.Motion.SetDeactivationClass(deadMode ? HkSolverDeactivation.High : HkSolverDeactivation.Medium);// - TODO: Find another way - this is deprecated by Havok
}
Ragdoll.OptimizeInertiasOfConstraintTree();
if (!Ragdoll.InWorld)
{
//Ragdoll.RecreateConstraints();
HavokWorld.AddRagdoll(Ragdoll);
}
Ragdoll.EnableConstraints();
Ragdoll.Activate();
m_ragdollDeadMode = deadMode;
if (MyFakes.ENABLE_RAGDOLL_DEBUG)
{
Debug.WriteLine("MyPhysicsBody.SwitchToRagdollMode - FINISHED");
MyLog.Default.WriteLine("MyPhysicsBody.SwitchToRagdollMode - FINISHED");
}
}
public void Unweld(MyPhysicsBody other, bool insertToWorld = true, bool recreateShape = true)
{
Debug.Assert(other.IsWelded && RigidBody != null && other.WeldedRigidBody != null, "Invalid welding state!");
if (IsWelded)
{
WeldInfo.Parent.Unweld(other, insertToWorld, recreateShape);
Debug.Assert(other.IsWelded);
return;
}
if (other.IsInWorld || RigidBody == null || other.WeldedRigidBody == null)
{
WeldInfo.Children.Remove(other);
return;
}
var rbWorldMatrix = RigidBody.GetRigidBodyMatrix();
//other.Entity.OnPhysicsChanged -= WeldedEntity_OnPhysicsChanged;
other.WeldInfo.Parent = null;
Debug.Assert(WeldInfo.Children.Contains(other));
WeldInfo.Children.Remove(other);
var body = other.RigidBody;
Debug.Assert(body == RigidBody);
other.RigidBody = other.WeldedRigidBody;
other.WeldedRigidBody = null;
if (!other.RigidBody.IsDisposed)
{
other.RigidBody.SetWorldMatrix(other.WeldInfo.Transform * rbWorldMatrix);
other.RigidBody.LinearVelocity = body.LinearVelocity;
other.WeldInfo.MassElement.Tranform = Matrix.Identity;
other.WeldInfo.Transform = Matrix.Identity;
}
else
{
Debug.Fail("Disposed welded body");
}
//RemoveConstraints(other.RigidBody);
other.ClusterObjectID = MyHavokCluster.CLUSTERED_OBJECT_ID_UNITIALIZED;
if (insertToWorld)
{
other.Activate();
other.OnMotion(other.RigidBody, 0);
}
if (WeldInfo.Children.Count == 0)
{
recreateShape = false;
Entity.OnPhysicsChanged -= WeldedEntity_OnPhysicsChanged;
Entity.OnClose -= Entity_OnClose;
WeldedRigidBody.LinearVelocity = RigidBody.LinearVelocity;
WeldedRigidBody.AngularVelocity = RigidBody.AngularVelocity;
if (HavokWorld != null)
{
HavokWorld.RemoveRigidBody(RigidBody);
}
RigidBody.Dispose();
RigidBody = WeldedRigidBody;
WeldedRigidBody = null;
RigidBody.SetWorldMatrix(rbWorldMatrix);
WeldInfo.Transform = Matrix.Identity;
if (HavokWorld != null)
{
HavokWorld.AddRigidBody(RigidBody);
}
else if (!Entity.MarkedForClose)
{
Activate();
}
if (RigidBody2 != null)
{
RigidBody2.SetShape(RigidBody.GetShape());
}
}
if (RigidBody != null && recreateShape)
{
RecreateWeldedShape(GetShape());
}
OnUnwelded(other);
other.OnUnwelded(this);
Debug.Assert(!other.IsWelded);
}
internal void UpdateAfterSimulation10()
{
UpdateRigidBodyShape();
// Apply prediction based on movement of nearby entities.
foreach (var entity in m_nearbyEntities)
{
if (!(entity is MyCubeGrid)) //jn:TODO prediction for lod0
{
continue;
}
if (entity.MarkedForClose)
{
continue;
}
if (entity.Physics.LinearVelocity.Length() < 2f)
{
continue;
}
var predictionOffset = ComputePredictionOffset(entity);
var aabb = entity.WorldAABB;
aabb.Inflate(MyVoxelConstants.GEOMETRY_CELL_SIZE_IN_METRES * 3);
aabb.Translate(predictionOffset);
if (!aabb.Intersects(m_voxelMap.PositionComp.WorldAABB))
{
continue;
}
Vector3I min, max;
Vector3D localPositionMin, localPositionMax;
MyVoxelCoordSystems.WorldPositionToLocalPosition(aabb.Min, m_voxelMap.PositionComp.WorldMatrix, m_voxelMap.PositionComp.WorldMatrixInvScaled, m_voxelMap.SizeInMetresHalf, out localPositionMin);
MyVoxelCoordSystems.WorldPositionToLocalPosition(aabb.Max, m_voxelMap.PositionComp.WorldMatrix, m_voxelMap.PositionComp.WorldMatrixInvScaled, m_voxelMap.SizeInMetresHalf, out localPositionMax);
MyVoxelCoordSystems.LocalPositionToVoxelCoord(ref localPositionMin, out min);
MyVoxelCoordSystems.LocalPositionToVoxelCoord(ref localPositionMax, out max);
m_voxelMap.Storage.ClampVoxelCoord(ref min);
m_voxelMap.Storage.ClampVoxelCoord(ref max);
MyVoxelCoordSystems.VoxelCoordToGeometryCellCoord(ref min, out min);
MyVoxelCoordSystems.VoxelCoordToGeometryCellCoord(ref max, out max);
{
var size = (max - min + 1).Size;
if (size >= m_cellsToGenerateBuffer.Length)
{
m_cellsToGenerateBuffer = new Vector3I[MathHelper.GetNearestBiggerPowerOfTwo(size)];
}
}
var shape = (HkUniformGridShape)GetShape();// RigidBody.GetShape();
Debug.Assert(shape.Base.IsValid);
int requiredCellsCount = shape.GetMissingCellsInRange(ref min, ref max, m_cellsToGenerateBuffer);
for (int i = 0; i < requiredCellsCount; ++i)
{
if (m_workTracker.Exists(m_cellsToGenerateBuffer[i]))
{
continue;
}
MyPrecalcJobPhysicsPrefetch.Start(new MyPrecalcJobPhysicsPrefetch.Args()
{
TargetPhysics = this,
Tracker = m_workTracker,
GeometryCell = new MyCellCoord(0, m_cellsToGenerateBuffer[i]),
Storage = m_voxelMap.Storage,
});
}
}
var voxelShape = (HkUniformGridShape)GetShape();
if (m_nearbyEntities.Count == 0 && RigidBody != null && MyFakes.ENABLE_VOXEL_PHYSICS_SHAPE_DISCARDING && voxelShape.ShapeCount > 0)
{
// RigidBody.GetShape();
Debug.Assert(voxelShape.Base.IsValid);
voxelShape.DiscardLargeData();
if (RigidBody2 != null)
{
voxelShape = (HkUniformGridShape)RigidBody2.GetShape();
voxelShape.DiscardLargeData();
}
}
}
private void CreateRigidBodies()
{
if (Entity.MarkedForClose)
{
return;
}
ProfilerShort.Begin("MyVoxelPhysicsBody::CreateRigidBodies()");
try
{
if (m_bodiesInitialized)
{
Debug.Fail("Double rigid body intialization for voxel map!");
return;
}
Vector3I numCels = m_voxelMap.Size >> MyVoxelConstants.GEOMETRY_CELL_SIZE_IN_VOXELS_BITS;
HkUniformGridShape shape;
HkRigidBody lod1rb = null;
if (MyFakes.USE_LOD1_VOXEL_PHYSICS)
{
shape = new HkUniformGridShape(
new HkUniformGridShapeArgs
{
CellsCount = numCels >> 1,
CellSize = MyVoxelConstants.GEOMETRY_CELL_SIZE_IN_METRES * 2,
CellOffset = MyVoxelConstants.VOXEL_SIZE_IN_METRES_HALF,
CellExpand = MyVoxelConstants.VOXEL_SIZE_IN_METRES
});
shape.SetShapeRequestHandler(RequestShapeBlockingLod1, QueryEmptyOrFull);
CreateFromCollisionObject(shape, -m_voxelMap.SizeInMetresHalf, m_voxelMap.WorldMatrix, collisionFilter: MyPhysics.CollisionLayers.VoxelLod1CollisionLayer);
shape.Base.RemoveReference();
lod1rb = RigidBody;
RigidBody = null;
}
shape = new HkUniformGridShape(
new HkUniformGridShapeArgs
{
CellsCount = numCels,
CellSize = MyVoxelConstants.GEOMETRY_CELL_SIZE_IN_METRES,
CellOffset = MyVoxelConstants.VOXEL_SIZE_IN_METRES_HALF,
CellExpand = MyVoxelConstants.VOXEL_SIZE_IN_METRES
});
shape.SetShapeRequestHandler(RequestShapeBlocking, QueryEmptyOrFull);
CreateFromCollisionObject(shape, -m_voxelMap.SizeInMetresHalf, m_voxelMap.WorldMatrix, collisionFilter: MyPhysics.CollisionLayers.VoxelCollisionLayer);
shape.Base.RemoveReference();
if (MyFakes.USE_LOD1_VOXEL_PHYSICS)
{
RigidBody2 = lod1rb;
}
if (MyFakes.ENABLE_PHYSICS_HIGH_FRICTION)
{
Friction = 0.65f;
}
m_bodiesInitialized = true;
// When doing the enable disable roundtrip we can mess up the ClusterTree because Activate() can end up calling this
// if the phantom is immediatelly intersecting something.
if (Enabled)
{
var matrix = GetRigidBodyMatrix();
RigidBody.SetWorldMatrix(matrix);
m_world.AddRigidBody(RigidBody);
if (MyFakes.USE_LOD1_VOXEL_PHYSICS)
{
RigidBody2.SetWorldMatrix(matrix);
m_world.AddRigidBody(RigidBody2);
}
}
}
finally
{
ProfilerShort.End();
}
}
