protected void UpdateUpdateable(SolverUpdateable item, float dt)
{
item.SolverSettings.currentIterations = 0;
item.SolverSettings.iterationsAtZeroImpulse = 0;
if (item.isActiveInSolver)
item.Update(dt);
}
/// <summary>
/// Solves a child updateable. Some children may override the group's update method;
/// this avoids code repeat.
/// </summary>
/// <param name="item"></param>
/// <param name="activeConstraints"> </param>
protected void SolveUpdateable(SolverUpdateable item, ref int activeConstraints)
{
if (item.isActiveInSolver)
{
SolverSettings subSolverSettings = item.solverSettings;
subSolverSettings.currentIterations++;
if (subSolverSettings.currentIterations <= solver.iterationLimit &&
subSolverSettings.currentIterations <= subSolverSettings.maximumIterationCount)
{
if (item.SolveIteration() < subSolverSettings.minimumImpulse)
{
subSolverSettings.iterationsAtZeroImpulse++;
if (subSolverSettings.iterationsAtZeroImpulse > subSolverSettings.minimumIterationCount)
{
item.isActiveInSolver = false;
}
else
{
activeConstraints++;
}
}
else
{
subSolverSettings.iterationsAtZeroImpulse = 0;
activeConstraints++;
}
}
else
{
item.isActiveInSolver = false;
}
}
}
///<summary>
/// Adds a constraint to the group.
///</summary>
///<param name="manifoldConstraint">Constraint to add.</param>
public new void Add(SolverUpdateable manifoldConstraint)
{
//This is a similar process to a normal solver group.
//However, it does not attempt to change involved entities.
//This is for two reasons:
//-It is unnecessary; a contact manifold is always between the same two entities throughout its lifespan.
//-It causes race conditions; this method is called in a multithreaded context and changing involved
// entities calls upon sequential-only methods.
if (manifoldConstraint.solver == null)
{
if (manifoldConstraint.SolverGroup == null)
{
solverUpdateables.Add(manifoldConstraint);
manifoldConstraint.SolverGroup = this;
manifoldConstraint.Solver = solver;
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a SolverGroup.");
}
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a solver.");
}
}
///<summary>
/// Adds a constraint to the group.
///</summary>
///<param name="manifoldConstraint">Constraint to add.</param>
public new void Add(SolverUpdateable manifoldConstraint)
{
//This is a similar process to a normal solver group.
//However, it does not attempt to change involved entities.
//This is for two reasons:
//-It is unnecessary; a contact manifold is always between the same two entities throughout its lifespan.
//-It causes race conditions; this method is called in a multithreaded context and changing involved
// entities calls upon sequential-only methods.
if (manifoldConstraint.solver == null)
{
if (manifoldConstraint.SolverGroup == null)
{
solverUpdateables.Add(manifoldConstraint);
manifoldConstraint.SolverGroup = this;
manifoldConstraint.Solver = solver;
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a SolverGroup.");
}
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a solver.");
}
}
protected void ExclusiveUpdateUpdateable(SolverUpdateable item)
{
if (item.isActiveInSolver)
{
item.ExclusiveUpdate();
}
}
protected void UpdateUpdateable(SolverUpdateable item, float dt)
{
item.SolverSettings.currentIterations = 0;
item.SolverSettings.iterationsAtZeroImpulse = 0;
if (item.isActiveInSolver)
{
item.Update(dt);
}
}
///<summary>
/// Enqueues a solver updateable created by some pair for flushing into the solver later.
///</summary>
///<param name="addedItem">Updateable to add.</param>
public void NotifyUpdateableAdded(SolverUpdateable addedItem)
{
if (ApplySolverUpdateableChangesDirectly)
{
Solver.Add(addedItem);
}
else
{
solverUpdateableChanges.Enqueue(new SolverUpdateableChange(true, addedItem));
}
}
///<summary>
/// Enqueues a solver updateable removed by some pair for flushing into the solver later.
///</summary>
///<param name="removedItem">Solver updateable to remove.</param>
public void NotifyUpdateableRemoved(SolverUpdateable removedItem)
{
if (ApplySolverUpdateableChangesDirectly)
{
Solver.Remove(removedItem);
}
else
{
solverUpdateableChanges.Enqueue(new SolverUpdateableChange(false, removedItem));
}
}
/// <summary>
/// Removes a solver updateable from the group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to remove.</param>
/// <exception cref="InvalidOperationException">Thrown when the SolverUpdateable to remove from the SolverGroup doesn't actually belong to this SolverGroup.</exception>
protected void Remove(SolverUpdateable solverUpdateable)
{
if (solverUpdateable.SolverGroup == this)
{
solverUpdateables.Remove(solverUpdateable);
solverUpdateable.SolverGroup = null;
solverUpdateable.Solver = null;
OnInvolvedEntitiesChanged();
}
else
{
throw new InvalidOperationException("Cannot remove SolverUpdateable from SolverGroup; it doesn't belong to this SolverGroup.");
}
}
void IPairHandlerParent.AddSolverUpdateable(SolverUpdateable addedItem)
{
manifoldConstraintGroup.Add(addedItem);
//If this is the first child solver item to be added, we need to add ourselves to our parent too.
if (manifoldConstraintGroup.SolverUpdateables.Count == 1)
{
if (Parent != null)
{
Parent.AddSolverUpdateable(manifoldConstraintGroup);
}
else if (NarrowPhase != null)
{
NarrowPhase.NotifyUpdateableAdded(manifoldConstraintGroup);
}
}
}
/// <summary>
/// Sets the activity state of the constraint based on the activity state of its connections.
/// Called automatically by the space owning a constaint. If a constraint is a sub-constraint that hasn't been directly added to the space,
/// this may need to be called alongside the preStep from within the parent constraint.
/// </summary>
public override void UpdateSolverActivity()
{
if (isActive)
{
isActiveInSolver = false;
for (int i = 0; i < solverUpdateables.Count; i++)
{
SolverUpdateable item = solverUpdateables.Elements[i];
item.UpdateSolverActivity();
isActiveInSolver |= item.isActiveInSolver;
}
}
else
{
isActiveInSolver = false;
}
}
void IPairHandlerParent.RemoveSolverUpdateable(SolverUpdateable removedItem)
{
manifoldConstraintGroup.Remove(removedItem);
//If this is the last child solver item, we need to remove ourselves from our parent too.
if (manifoldConstraintGroup.SolverUpdateables.Count == 0)
{
if (Parent != null)
{
Parent.RemoveSolverUpdateable(manifoldConstraintGroup);
}
else if (NarrowPhase != null)
{
NarrowPhase.NotifyUpdateableRemoved(manifoldConstraintGroup);
}
}
}
/// <summary>
/// Adds a solver updateable to the group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to add.</param>
/// <exception cref="InvalidOperationException">Thrown when the SolverUpdateable to add to the SolverGroup already belongs to another SolverGroup or to a Space.</exception>
protected void Add(SolverUpdateable solverUpdateable)
{
if (solverUpdateable.solver == null)
{
if (solverUpdateable.SolverGroup == null)
{
solverUpdateables.Add(solverUpdateable);
solverUpdateable.SolverGroup = this;
solverUpdateable.Solver = solver;
OnInvolvedEntitiesChanged();
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a SolverGroup.");
}
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a solver.");
}
}
///<summary>
/// Constructs a new solver updateable change.
///</summary>
///<param name="shouldAdd">Whether the item is going to be added or removed.</param>
///<param name="item">Item to add or remove.</param>
public SolverUpdateableChange(bool shouldAdd, SolverUpdateable item)
{
ShouldAdd = shouldAdd;
Item = item;
}
protected internal void UnsafeExclusiveUpdate(SolverUpdateable updateable)
{
if (updateable.isActiveInSolver)
{
updateable.ExclusiveUpdate();
}
}
public MotorSettingsOrientation(SolverUpdateable motor)
: base(motor)
{
servo = new ServoSettingsOrientation(this);
velocityMotor = new VelocityMotorSettings3D(this);
}
/// <summary>
/// Adds a solver updateable to the group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to add.</param>
/// <exception cref="InvalidOperationException">Thrown when the SolverUpdateable to add to the SolverGroup already belongs to another SolverGroup or to a Space.</exception>
protected void Add(SolverUpdateable solverUpdateable)
{
if (solverUpdateable.solver == null)
{
if (solverUpdateable.SolverGroup == null)
{
solverUpdateables.Add(solverUpdateable);
solverUpdateable.SolverGroup = this;
solverUpdateable.Solver = solver;
OnInvolvedEntitiesChanged();
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a SolverGroup.");
}
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a solver.");
}
}
protected internal void UnsafeSolveIteration(SolverUpdateable updateable)
{
if (updateable.isActiveInSolver)
{
SolverSettings solverSettings = updateable.solverSettings;
solverSettings.currentIterations++;
if (solverSettings.currentIterations <= iterationLimit &&
solverSettings.currentIterations <= solverSettings.maximumIterationCount)
{
if (updateable.SolveIteration() < solverSettings.minimumImpulse)
{
solverSettings.iterationsAtZeroImpulse++;
if (solverSettings.iterationsAtZeroImpulse > solverSettings.minimumIterationCount)
updateable.isActiveInSolver = false;
}
else
{
solverSettings.iterationsAtZeroImpulse = 0;
}
}
else
{
updateable.isActiveInSolver = false;
}
}
}
///<summary>
/// Enqueues a solver updateable created by some pair for flushing into the solver later.
///</summary>
///<param name="addedItem">Updateable to add.</param>
public void NotifyUpdateableAdded(SolverUpdateable addedItem)
{
Solver.Add(addedItem);
}
public void Add(SolverUpdateable joint)
{
_joints.Add(joint);
}
/// <summary>
/// Adds a new solver updateable to the solver group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to add.</param>
public new void Add(SolverUpdateable solverUpdateable)
{
base.Add(solverUpdateable);
}
void IPairHandlerParent.AddSolverUpdateable(SolverUpdateable addedItem)
{
manifoldConstraintGroup.Add(addedItem);
//If this is the first child solver item to be added, we need to add ourselves to our parent too.
if (manifoldConstraintGroup.SolverUpdateables.Count == 1)
{
if (Parent != null)
Parent.AddSolverUpdateable(manifoldConstraintGroup);
else if (NarrowPhase != null)
NarrowPhase.NotifyUpdateableAdded(manifoldConstraintGroup);
}
}
public MotorSettings3D(SolverUpdateable motor)
: base(motor)
{
servo = new ServoSettings3D(this);
velocityMotor = new VelocityMotorSettings3D(this);
}
protected MotorSettings(SolverUpdateable motor)
{
this.motor = motor;
}
protected void ExclusiveUpdateUpdateable(SolverUpdateable item)
{
if (item.isActiveInSolver)
item.ExclusiveUpdate();
}
void IPairHandlerParent.RemoveSolverUpdateable(SolverUpdateable removedItem)
{
manifoldConstraintGroup.Remove(removedItem);
//If this is the last child solver item, we need to remove ourselves from our parent too.
if (manifoldConstraintGroup.SolverUpdateables.Count == 0)
{
if (Parent != null)
Parent.RemoveSolverUpdateable(manifoldConstraintGroup);
else if (NarrowPhase != null)
NarrowPhase.NotifyUpdateableRemoved(manifoldConstraintGroup);
}
}
/// <summary>
/// Removes a solver updateable from the group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to remove.</param>
/// <exception cref="InvalidOperationException">Thrown when the SolverUpdateable to remove from the SolverGroup doesn't actually belong to this SolverGroup.</exception>
protected void Remove(SolverUpdateable solverUpdateable)
{
if (solverUpdateable.SolverGroup == this)
{
solverUpdateables.Remove(solverUpdateable);
solverUpdateable.SolverGroup = null;
solverUpdateable.Solver = null;
OnInvolvedEntitiesChanged();
}
else
{
throw new InvalidOperationException("Cannot remove SolverUpdateable from SolverGroup; it doesn't belong to this SolverGroup.");
}
}
/// <summary>
/// Adds a new solver updateable to the solver group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to add.</param>
public new void Add(SolverUpdateable solverUpdateable)
{
base.Add(solverUpdateable);
}
///<summary>
/// Removes a solver updateable from the solver.
///</summary>
///<param name="item">Updateable to remove.</param>
///<exception cref="ArgumentException">Thrown when the item does not belong to the solver.</exception>
public void Remove(SolverUpdateable item)
{
if (item.Solver == this)
{
item.Solver = null;
addRemoveLocker.Enter();
solverUpdateables.Count--;
if (item.solverIndex < solverUpdateables.Count)
{
//The solver updateable isn't the last element, so put the last element in its place.
solverUpdateables.Elements[item.solverIndex] = solverUpdateables.Elements[solverUpdateables.Count];
//Update the replacement's solver index to its new location.
solverUpdateables.Elements[item.solverIndex].solverIndex = item.solverIndex;
}
solverUpdateables.Elements[solverUpdateables.Count] = null;
addRemoveLocker.Exit();
DeactivationManager.Remove(item.simulationIslandConnection);
item.OnRemovalFromSolver(this);
}
else
throw new ArgumentException("Solver updateable doesn't belong to this solver; it can't be removed.", "item");
}
/// <summary>
/// Removes a solver updateable from the solver group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to remove.</param>
public new void Remove(SolverUpdateable solverUpdateable)
{
base.Remove(solverUpdateable);
}
internal MotorSettingsOrientation(SolverUpdateable motor)
: base(motor)
{
servo = new ServoSettingsOrientation(this);
velocityMotor = new VelocityMotorSettings3D(this);
}
///<summary>
/// Enqueues a solver updateable removed by some pair for flushing into the solver later.
///</summary>
///<param name="removedItem">Solver updateable to remove.</param>
public void NotifyUpdateableRemoved(SolverUpdateable removedItem)
{
Solver.Remove(removedItem);
}
///<summary>
/// Removes a space object from the simulation.
///</summary>
///<param name="spaceObject">Space object to remove.</param>
public void Remove(ISpaceObject spaceObject)
{
if (spaceObject.Space != this)
{
throw new ArgumentException("The object does not belong to this space; cannot remove it.");
}
SimulationIslandMember simulationIslandMember = spaceObject as SimulationIslandMember;
if (simulationIslandMember != null)
{
DeactivationManager.Remove(simulationIslandMember);
}
ISimulationIslandMemberOwner simulationIslandMemberOwner = spaceObject as ISimulationIslandMemberOwner;
if (simulationIslandMemberOwner != null)
{
DeactivationManager.Remove(simulationIslandMemberOwner.ActivityInformation);
}
//Go through each stage, removing the space object from it if necessary.
IForceUpdateable velocityUpdateable = spaceObject as IForceUpdateable;
if (velocityUpdateable != null)
{
ForceUpdater.Remove(velocityUpdateable);
}
MobileCollidable boundingBoxUpdateable = spaceObject as MobileCollidable;
if (boundingBoxUpdateable != null)
{
BoundingBoxUpdater.Remove(boundingBoxUpdateable);
}
BroadPhaseEntry broadPhaseEntry = spaceObject as BroadPhaseEntry;
if (broadPhaseEntry != null)
{
BroadPhase.Remove(broadPhaseEntry);
}
//Entites own collision proxies, but are not entries themselves.
IBroadPhaseEntryOwner broadPhaseEntryOwner = spaceObject as IBroadPhaseEntryOwner;
if (broadPhaseEntryOwner != null)
{
BroadPhase.Remove(broadPhaseEntryOwner.Entry);
boundingBoxUpdateable = broadPhaseEntryOwner.Entry as MobileCollidable;
if (boundingBoxUpdateable != null)
{
BoundingBoxUpdater.Remove(boundingBoxUpdateable);
}
}
SolverUpdateable solverUpdateable = spaceObject as SolverUpdateable;
if (solverUpdateable != null)
{
Solver.Remove(solverUpdateable);
}
IPositionUpdateable integrable = spaceObject as IPositionUpdateable;
if (integrable != null)
{
PositionUpdater.Remove(integrable);
}
Entity entity = spaceObject as Entity;
if (entity != null)
{
BufferedStates.Remove(entity);
}
IDeferredEventCreator deferredEventCreator = spaceObject as IDeferredEventCreator;
if (deferredEventCreator != null)
{
DeferredEventDispatcher.RemoveEventCreator(deferredEventCreator);
}
IDeferredEventCreatorOwner deferredEventCreatorOwner = spaceObject as IDeferredEventCreatorOwner;
if (deferredEventCreatorOwner != null)
{
DeferredEventDispatcher.RemoveEventCreator(deferredEventCreatorOwner.EventCreator);
}
//Updateable stages.
IDuringForcesUpdateable duringForcesUpdateable = spaceObject as IDuringForcesUpdateable;
if (duringForcesUpdateable != null)
{
DuringForcesUpdateables.Remove(duringForcesUpdateable);
}
IBeforeNarrowPhaseUpdateable beforeNarrowPhaseUpdateable = spaceObject as IBeforeNarrowPhaseUpdateable;
if (beforeNarrowPhaseUpdateable != null)
{
BeforeNarrowPhaseUpdateables.Remove(beforeNarrowPhaseUpdateable);
}
IBeforeSolverUpdateable beforeSolverUpdateable = spaceObject as IBeforeSolverUpdateable;
if (beforeSolverUpdateable != null)
{
BeforeSolverUpdateables.Remove(beforeSolverUpdateable);
}
IBeforePositionUpdateUpdateable beforePositionUpdateUpdateable = spaceObject as IBeforePositionUpdateUpdateable;
if (beforePositionUpdateUpdateable != null)
{
BeforePositionUpdateUpdateables.Remove(beforePositionUpdateUpdateable);
}
IEndOfTimeStepUpdateable endOfStepUpdateable = spaceObject as IEndOfTimeStepUpdateable;
if (endOfStepUpdateable != null)
{
EndOfTimeStepUpdateables.Remove(endOfStepUpdateable);
}
IEndOfFrameUpdateable endOfFrameUpdateable = spaceObject as IEndOfFrameUpdateable;
if (endOfFrameUpdateable != null)
{
EndOfFrameUpdateables.Remove(endOfFrameUpdateable);
}
spaceObject.Space = null;
spaceObject.OnRemovalFromSpace(this);
}
///<summary>
/// Enqueues a solver updateable removed by some pair for flushing into the solver later.
///</summary>
///<param name="removedItem">Solver updateable to remove.</param>
public void NotifyUpdateableRemoved(SolverUpdateable removedItem)
{
solverUpdateableChanges.Enqueue(new SolverUpdateableChange(false, removedItem));
}
/// <summary>
/// Solves a child updateable. Some children may override the group's update method;
/// this avoids code repeat.
/// </summary>
/// <param name="item"></param>
/// <param name="activeConstraints"> </param>
protected void SolveUpdateable(SolverUpdateable item, ref int activeConstraints)
{
if (item.isActiveInSolver)
{
SolverSettings subSolverSettings = item.solverSettings;
subSolverSettings.currentIterations++;
if (subSolverSettings.currentIterations <= solver.iterationLimit &&
subSolverSettings.currentIterations <= subSolverSettings.maximumIterationCount)
{
if (item.SolveIteration() < subSolverSettings.minimumImpulse)
{
subSolverSettings.iterationsAtZeroImpulse++;
if (subSolverSettings.iterationsAtZeroImpulse > subSolverSettings.minimumIterationCount)
item.isActiveInSolver = false;
else
{
activeConstraints++;
}
}
else
{
subSolverSettings.iterationsAtZeroImpulse = 0;
activeConstraints++;
}
}
else
{
item.isActiveInSolver = false;
}
}
}
internal MotorSettings(SolverUpdateable motor)
{
this.motor = motor;
}
///<summary>
/// Constructs a new solver updateable change.
///</summary>
///<param name="shouldAdd">Whether the item is going to be added or removed.</param>
///<param name="item">Item to add or remove.</param>
public SolverUpdateableChange(bool shouldAdd, SolverUpdateable item)
{
ShouldAdd = shouldAdd;
Item = item;
}
//--------------------------------------------------------------------------
#region Joints
static public void RemoveJoint(SolverUpdateable joint)
{
World.Remove(joint);
}
internal MotorSettings(SolverUpdateable motor)
{
this.motor = motor;
}
protected internal void UnsafePrestep(SolverUpdateable updateable)
{
updateable.UpdateSolverActivity();
if (updateable.isActiveInSolver)
{
SolverSettings solverSettings = updateable.solverSettings;
solverSettings.currentIterations = 0;
solverSettings.iterationsAtZeroImpulse = 0;
updateable.Update(timeStepSettings.TimeStepDuration);
}
}
internal MotorSettings3D(SolverUpdateable motor)
: base(motor)
{
servo = new ServoSettings3D(this);
velocityMotor = new VelocityMotorSettings3D(this);
}
/// <summary>
/// Removes a solver updateable from the solver group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to remove.</param>
public new void Remove(SolverUpdateable solverUpdateable)
{
base.Remove(solverUpdateable);
}
protected MotorSettings(SolverUpdateable motor)
{
this.motor = motor;
}
///<summary>
/// Enqueues a solver updateable created by some pair for flushing into the solver later.
///</summary>
///<param name="addedItem">Updateable to add.</param>
public void NotifyUpdateableAdded(SolverUpdateable addedItem)
{
solverUpdateableChanges.Enqueue(new SolverUpdateableChange(true, addedItem));
}
///<summary>
/// Adds a solver updateable to the solver.
///</summary>
///<param name="item">Updateable to add.</param>
///<exception cref="ArgumentException">Thrown when the item already belongs to a solver.</exception>
public void Add(SolverUpdateable item)
{
if (item.Solver == null)
{
item.Solver = this;
addRemoveLocker.Enter();
item.solverIndex = solverUpdateables.Count;
solverUpdateables.Add(item);
addRemoveLocker.Exit();
DeactivationManager.Add(item.simulationIslandConnection);
item.OnAdditionToSolver(this);
}
else
throw new ArgumentException("Solver updateable already belongs to something; it can't be added.", "item");
}
