private static void GetDesiredVector(out Vector3D unit, out double length, ChasePoint_GetForceArgs e, ChaseDirectionType direction)
{
switch (direction)
{
case ChaseDirectionType.Drag_Velocity_Along:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityAway:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityToward:
unit = e.VelocityAlongUnit;
length = e.VelocityAlongLength;
break;
case ChaseDirectionType.Attract_Direction:
unit = e.DirectionUnit;
length = e.DirectionLength;
break;
case ChaseDirectionType.Drag_Velocity_Any:
unit = e.VelocityUnit;
length = e.VelocityLength;
break;
case ChaseDirectionType.Drag_Velocity_Orth:
unit = e.VelocityOrthUnit;
length = e.VelocityOrthLength;
break;
default:
throw new ApplicationException("Unknown DirectionType: " + direction.ToString());
}
}
private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
{
// See if there is anything to do
if (_desiredPoint == null)
{
return;
}
//NOTE: Offset needs to be center of mass
Point3D current = e.Body.PositionToWorld(e.Body.CenterOfMass + this.Offset);
ChasePoint_GetForceArgs args = new ChasePoint_GetForceArgs(this.Item, _desiredPoint.Value - current);
Vector3D?force = GetForce(args, this.Forces, this.Percent, this.MaxForce, this.MaxAcceleration);
if (force != null && !force.Value.IsNearZero())
{
if (_shouldCauseTorque)
{
e.Body.AddForceAtPoint(force.Value, current);
}
else
{
e.Body.AddForce(force.Value);
}
}
}
public static Vector3D?GetForce(ChasePoint_GetForceArgs args, IEnumerable <ChasePoint_Force> workers, double percent = 1d, double?maxForce = null, double?maxAcceleration = null)
{
Vector3D?retVal = null;
// Call each worker
foreach (var worker in workers)
{
Vector3D?localForce = worker.GetForce(args);
if (localForce != null)
{
if (retVal == null)
{
retVal = localForce;
}
else
{
retVal = retVal.Value + localForce.Value;
}
}
}
if (retVal != null)
{
retVal = retVal.Value * percent;
}
// Apply the force
if (retVal != null && !retVal.Value.IsNearZero())
{
// Limit if exceeds this.MaxForce
if (maxForce != null && retVal.Value.LengthSquared > maxForce.Value * maxForce.Value)
{
retVal = retVal.Value.ToUnit() * maxForce.Value;
}
// Limit acceleration
if (maxAcceleration != null)
{
//f=ma
double accel = retVal.Value.Length / args.ItemMass;
if (accel > maxAcceleration.Value)
{
retVal = retVal.Value.ToUnit() * (maxAcceleration.Value * args.ItemMass);
}
}
}
return(retVal);
}
public Vector3D?GetForce(ChasePoint_GetForceArgs e)
{
if (!IsDirectionValid(e.IsVelocityAlongTowards))
{
return(null);
}
GetDesiredVector(out Vector3D unit, out double length, e, this.Direction);
if (Math3D.IsNearZero(unit))
{
return(null);
}
double force = this.Value;
if (this.IsAccel)
{
// f=ma
force *= e.ItemMass;
}
if (this.IsSpring)
{
force *= GetDistance(e.DirectionLength, e.Item.Radius);
}
if (this.IsDrag)
{
force *= -length; // negative, because it needs to be a drag force
}
// Gradient %
if (this.Gradient != null)
{
force *= GetGradientPercent(GetDistance(e.DirectionLength, e.Item.Radius), Gradient);
}
return(unit * force);
}
private static Vector3D? GetNeighborAccel(IMapObject thisBot, Tuple<MapObjectInfo, double, ForceSettings_Initial>[] neighbors, Point3D position, Vector3D velocity)
{
Vector3D? retVal = null;
foreach (var neighbor in neighbors)
{
#region attract/repel
ChasePoint_GetForceArgs args = new ChasePoint_GetForceArgs(thisBot, neighbor.Item1.Position - position);
Vector3D? attractRepelAccel = MapObject_ChasePoint_Forces.GetForce(args, neighbor.Item3.Forces);
#endregion
#region match velocity
Vector3D? accel = null;
if (neighbor.Item3.MatchVelocityPercent != null)
{
Vector3D matchVelocity = GetMatchVelocityForce(neighbor.Item1, velocity);
// Combine forces
if (attractRepelAccel == null)
{
accel = matchVelocity;
}
else
{
accel = Math3D.GetAverage(new[]
{
Tuple.Create(attractRepelAccel.Value, 1d),
Tuple.Create(matchVelocity, neighbor.Item3.MatchVelocityPercent.Value) //NOTE: When the percent is 1 (100%), this will cause a 50/50 average with the other accel
});
}
}
else
{
accel = attractRepelAccel;
}
#endregion
// Add to total
if (accel != null)
{
if (retVal == null)
{
retVal = accel;
}
else
{
retVal = retVal.Value + accel.Value;
}
}
}
return retVal;
}
private static void GetDesiredVector(out Vector3D unit, out double length, ChasePoint_GetForceArgs e, ChaseDirectionType direction)
{
switch (direction)
{
case ChaseDirectionType.Drag_Velocity_Along:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityAway:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityToward:
unit = e.VelocityAlongUnit;
length = e.VelocityAlongLength;
break;
case ChaseDirectionType.Attract_Direction:
unit = e.DirectionUnit;
length = e.DirectionLength;
break;
case ChaseDirectionType.Drag_Velocity_Any:
unit = e.VelocityUnit;
length = e.VelocityLength;
break;
case ChaseDirectionType.Drag_Velocity_Orth:
unit = e.VelocityOrthUnit;
length = e.VelocityOrthLength;
break;
default:
throw new ApplicationException("Unknown DirectionType: " + direction.ToString());
}
}
public Vector3D? GetForce(ChasePoint_GetForceArgs e)
{
if (!IsDirectionValid(e.IsVelocityAlongTowards))
{
return null;
}
Vector3D unit;
double length;
GetDesiredVector(out unit, out length, e, this.Direction);
if (Math3D.IsNearZero(unit))
{
return null;
}
double force = this.Value;
if (this.IsAccel)
{
// f=ma
force *= e.ItemMass;
}
if (this.IsSpring)
{
force *= GetDistance(e.DirectionLength, e.Item.Radius);
}
if (this.IsDrag)
{
force *= -length; // negative, because it needs to be a drag force
}
// Gradient %
if (this.Gradient != null)
{
force *= GetGradientPercent(GetDistance(e.DirectionLength, e.Item.Radius), this.Gradient);
}
return unit * force;
}
private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
{
// See if there is anything to do
if (_desiredPoint == null)
{
return;
}
//NOTE: Offset needs to be center of mass
Point3D current = e.Body.PositionToWorld(e.Body.CenterOfMass + this.Offset);
ChasePoint_GetForceArgs args = new ChasePoint_GetForceArgs(this.Item, _desiredPoint.Value - current);
Vector3D? force = GetForce(args, this.Forces, this.Percent, this.MaxForce, this.MaxAcceleration);
if (force != null && !force.Value.IsNearZero())
{
if (_shouldCauseTorque)
{
e.Body.AddForceAtPoint(force.Value, current);
}
else
{
e.Body.AddForce(force.Value);
}
}
}
public static Vector3D? GetForce(ChasePoint_GetForceArgs args, IEnumerable<ChasePoint_Force> workers, double percent = 1d, double? maxForce = null, double? maxAcceleration = null)
{
Vector3D? retVal = null;
// Call each worker
foreach (var worker in workers)
{
Vector3D? localForce = worker.GetForce(args);
if (localForce != null)
{
if (retVal == null)
{
retVal = localForce;
}
else
{
retVal = retVal.Value + localForce.Value;
}
}
}
if (retVal != null)
{
retVal = retVal.Value * percent;
}
// Apply the force
if (retVal != null && !retVal.Value.IsNearZero())
{
// Limit if exceeds this.MaxForce
if (maxForce != null && retVal.Value.LengthSquared > maxForce.Value * maxForce.Value)
{
retVal = retVal.Value.ToUnit(false) * maxForce.Value;
}
// Limit acceleration
if (maxAcceleration != null)
{
//f=ma
double accel = retVal.Value.Length / args.ItemMass;
if (accel > maxAcceleration.Value)
{
retVal = retVal.Value.ToUnit(false) * (maxAcceleration.Value * args.ItemMass);
}
}
}
return retVal;
}
