public static Vector2 Arrive(MovingEntity vehicle, Deceleration deceleration)
{
//arrive = true; // NEEDS ATTENTION
float distance = Vector2.Distance(vehicle.targetPos, vehicle.position);
// savedDeceleration = (double)deceleration;
if (distance < 0.001)
{
return(Vector2.zero);
}
else if (distance > 0)
{
double decelerationTweaker = 0.2;
double speed = distance / ((double)deceleration * decelerationTweaker);
speed = Mathf.Min((float)speed, vehicle.maxSpeed); // don't exceed max speed
Vector2 desiredVelocity = (vehicle.targetPos - vehicle.position) * (float)speed / distance;
return(desiredVelocity - vehicle.velocity);
}
return(Vector2.zero);
}
Vector2 Arrive(Vector2 target, Deceleration decel)
{
Vector2 toTarget = target - (Vector2)player.transform.position;
//calculate the distance to the target
float dist = toTarget.magnitude;
if (dist > 0)
{
//because Deceleration is enumerated as an int, this value is required
//to provide fine tweaking of the deceleration..
const float DecelerationTweaker = 0.3f;
//calculate the speed required to reach the target given the desired
//deceleration
float speed = dist / ((float)decel * DecelerationTweaker);
//make sure the velocity does not exceed the max
speed = Mathf.Min(speed, player.myMaxSpeed);
//from here proceed just like Seek except we don't need to normalize
//the ToTarget vector because we have already gone to the trouble
//of calculating its length: dist.
Vector2 desiredVelocity = toTarget * speed / dist;
return(desiredVelocity - player.Velocity());
}
return(new Vector2(0f, 0f));
}
/********************** Arrive *************************/
private Vector3 Arrive(Vector3 targetPos, Deceleration deceleration)
{
Vector3 toTarget = targetPos - myVehicle.Position();
double distance = toTarget.magnitude;
// if hasn't arrived at the destination
if (distance > 0)
{
const float tweaker = 0.3f;
double speed = distance / ((double)deceleration * tweaker);
// limit the speed with the max speed
speed = Mathf.Min((float)speed, myVehicle.maxSpeed);
// get the desired velocity
Vector3 desiredVelocity = toTarget * (float)speed / (float)distance;
Debug.Log("arrive force " + (desiredVelocity - myVehicle.GetVelocity()));
return(desiredVelocity - myVehicle.GetVelocity());
}
return(new Vector3(0f, 0f, 0f));
}
//Arrive
public Vector2 Arrive(Vector2 targetPos, Deceleration deceleration)
{
Vector2 ToTarget = targetPos - _actor.Position;
//calculate the distance to the target position
float dist = ToTarget.Length();
if (dist > 0)
{
//because Deceleration is enumerated as an int, this value is required
//to provide fine tweaking of the deceleration.
const float DecelerationTweaker = 0.3f;
//calculate the speed required to reach the target given the desired
//deceleration
float speed = dist / ((float)deceleration * DecelerationTweaker);
//make sure the velocity does not exceed the max
speed = Math.Min(speed, _actor.MaxSpeed);
//from here proceed just like Seek except we don't need to normalize
//the ToTarget vector because we have already gone to the trouble
//of calculating its length: dist.
Vector2 DesiredVelocity = ToTarget * speed / dist;
return(DesiredVelocity - _actor.Velocity);
}
return(Vector2.Zero);
}
//-------------------------------------------------------------------------
// Wander Behaviour
//-------------------------------------------------------------------------
/*public Vector2 wander(){
*
* Vector2 targetPosition = wanderSteeringDirection - (Vector2)transform.forward;
* targetPosition = new Vector2 (targetPosition.x + Random.Range(-wanderMaxJitter, wanderMaxJitter)*Time.deltaTime, targetPosition.y + Random.Range(-wanderMaxJitter, wanderMaxJitter)*Time.deltaTime);
* targetPosition = targetPosition.normalized;
* targetPosition *= wanderCircleRadius;
*
* wanderSteeringDirection = targetPosition + (Vector2)transform.forward * wanderCircleOffset;
*
* return wanderSteeringDirection;
* }*/
//-------------------------------------------------------------------------
// Arrive Behaviour
//-------------------------------------------------------------------------
public Vector2 arrive(Vector3 arriveTargetPosition, Deceleration decelerationRate)
{
Vector2 toTarget = arriveTargetPosition - rigidbody2D.transform.position;
//calculate the remaining distance to the arrive target
float distanceToCover = toTarget.magnitude;
if (distanceToCover > 0)
{
//Because the Deceleration is int based enum
//This value is used to tweak the deceleration as required.
float deceleratoinTweaker = 0.3f;
//calculate the required speed with the given deceleration
float requiredSpeed = distanceToCover / (float)((float)decelerationRate * deceleratoinTweaker);
//make sure the requiredSpeed calculated does not exceed the max spped
requiredSpeed = Mathf.Min(maxSpeed, requiredSpeed);
//Desired velocity is calculated just like the seek.
//here we are not normalizing... cause we already calculated the distance to cover.
//just divide by it to normalize... to lower the number of calculation.
Vector2 desiredVelocity = toTarget * requiredSpeed / distanceToCover;
return(desiredVelocity - rigidbody2D.velocity);
}
//If distance is almost zero... then we have arrived the destination
return(new Vector2(0, 0));
}
/**
* This behavior is similar to seek but it attempts to arrive at the
* target with a zero velocity
*/
private Vector3 Arrive(Vector3 TargetPos, Deceleration deceleration)
{
Vector3 ToTarget = TargetPos - m_entity.VPos();
//calculate the distance to the target
float dist = ToTarget.magnitude;
if (dist > 0)
{
//because Deceleration is enumerated as an int, this value is required
//to provide fine tweaking of the deceleration..
float DecelerationTweaker = 0.3f;
//calculate the speed required to reach the target given the desired
//deceleration
float speed = dist / ((float)deceleration * DecelerationTweaker);
//make sure the velocity does not exceed the max
speed = Mathf.Min(speed, m_entity.MaxSpeed());
//from here proceed just like Seek except we don't need to normalize
//the ToTarget vector because we have already gone to the trouble
//of calculating its length: dist.
Vector3 DesiredVelocity = ToTarget * (speed / dist);
return(DesiredVelocity - m_entity.Velocity());
}
return(Vector3.zero);
}
public SteeringBehavior(Vehicle vehicle)
{
this.vehicle = vehicle;
this.target = new Vector3(Vector3.zero);
this.targetAgent = null;
deceleration = Deceleration.normal;
wanderTarget = new Vector3(2.0f, 0.0f, 0.0f);
}
public Steering(Vehicle vehicle)
{
this.vehicle = vehicle;
steeringForce = new Vector3(0f, 0f, 0f);
wanderTarget = new Vector3(0f, 0f, 0f);
deceleration = Deceleration.fast;
initSteerParam();
ObstacleAvoidanceOn();
WallAvoidanceOn();
}
public FieldbusInputData()
{
Control_I1 = new ControlWordI1();
BinaryOut_I2 = new BinaryOutputs_I2();
Control_I3 = new ControlWordI3();
SetpointVelocity = new SetpointVelocity();
Acceleration = new Acceleration();
Deceleration = new Deceleration();
Setpoint_Position = new SetpointPosition();
Subcontrol = new SubcontrolWord();
BinaryOut_I10 = new BinaryOutputsI10();
SetpointValue = new SetpointValue2();
}
/// <summary>
/// 抵达
/// </summary>
/// <param name="target"></param>
/// <param name="deceleration"></param>
/// <returns></returns>
public Vector2 Arrive(Vector2 target, Deceleration deceleration)
{
Vector2 ToTarget = target - m_vehicle.position;
float dist = ToTarget.magnitude;
if (dist > 0.1)
{
float decelerationTweaker = 0.5f;
float speed = Mathf.Min(dist / ((int)deceleration * decelerationTweaker), m_vehicle.m_MaxSpeed);//TODO：调参
Vector2 desiredVelocity = ToTarget * speed / dist;
return((desiredVelocity - m_vehicle.velocity).normalized * m_vehicle.m_MaxForce);
}
return(new Vector2(0, 0));
}
public Vector2 Arrive(Vector2 targetPosition, Deceleration deceleration)
{
var toTarget = (targetPosition - this._boid.Position);
var distanceToTarget = toTarget.magnitude;
if (distanceToTarget > Mathf.Epsilon)
{
var speed = distanceToTarget / ((float)deceleration * DECELERATION_TWEAKER);
speed = Mathf.Min(speed, this._boid.MaxSpeed);
var desiredVelocity = toTarget * speed / distanceToTarget;
return(this._boid.Mass * (desiredVelocity - this._boid.Velocity));
}
return(this._boid.Mass * (Vector2.zero - this._boid.Velocity));
}
/// <summary>
/// 到达
/// </summary>
/// <param name="_targetPos"></param>
/// <returns></returns>
Vector2 Arrive(Vector2 _targetPos, Deceleration _decel)
{
Vector2 toTarget = _targetPos - m_entity.Position;
float distance = toTarget.magnitude;
if (distance > 0.1f)
{
float decelTweaker = 0.3f;
float speed = distance / ((int)_decel * decelTweaker);
speed = Math.Min(speed, m_entity.MaxVelocity);
Vector2 desiredVolecity = toTarget * speed / speed;
return(desiredVolecity - m_entity.Velocity);
}
return(Vector2.zero);
}
public static XElement WriteDecelerationXML(Artifact item, DateTime AbsoluteStart)
{
Deceleration decItem = item.ArtifactData as Deceleration;
int decelType = ArtifactsHelper.GetDecelType(item);
var value = new XElement("event",
new XAttribute("start", ((int)((item.StartTime - AbsoluteStart).TotalSeconds * 4)).ToString(CultureInfo.InvariantCulture)),
new XAttribute("peak", ((int)((decItem.PeakTime - AbsoluteStart).TotalSeconds * 4)).ToString(CultureInfo.InvariantCulture)),
new XAttribute("end", ((int)((item.EndTime - AbsoluteStart).TotalSeconds * 4)).ToString(CultureInfo.InvariantCulture)),
new XAttribute("contraction", "-1"),
new XAttribute("type", decelType.ToString()),
new XAttribute("y1", "0.000"),
new XAttribute("y2", "0.000"));
return(value);
}
/// <summary>
/// 向目标位置移动，在即将抵达目标位置时减速
/// </summary>
public Vector2 Arrive(Vector2 targetPos, Deceleration deceleration)
{
Vector2 toTarget = targetPos - vehicle.pos;
float dist = toTarget.magnitude;
if (dist > 0f)
{
const float DecelerationTweaker = 0.3f;
float speed = dist / ((float)deceleration * DecelerationTweaker);
speed = Mathf.Min(speed, vehicle.maxSpeed);
Vector2 desiredVelocity = toTarget * speed / dist;
return(desiredVelocity - vehicle.velocity);
}
return(Vector2.zero);
}
private Vector3 Arrive(Vector3 target, Deceleration deceleration)
{
Vector3 toTarget = target - transform.position;
float dist = toTarget.magnitude;
if (dist > aiManager.SteeringSettings.ArriveRadius)
{
float speed = dist / ((float)deceleration * aiManager.SteeringSettings.DecelerationTweaker);
speed = Mathf.Min(speed, Vehicle.MaxSpeed);
Vector3 desiredVelocity = toTarget * speed / dist;
return(desiredVelocity - Vehicle.RB.velocity);
}
return(Vector3.zero);
}
public Vector3 Arrive(Vector3 t, float stopDistance, Deceleration deceleration)
{
Vector3 ToTarget = t - transform.position;
float distance = ToTarget.magnitude;
if (distance > stopDistance)
{
float decelerationAdjustment = 0.3f;
float speed = distance / (((float)deceleration + 0.1f) * decelerationAdjustment);
speed = Mathf.Min(speed, MaxVelocity);
Vector3 desiredVelocity = ToTarget * speed / distance;
return(desiredVelocity - velocity);
}
return(Vector3.zero);
}
private Vector2 Arrive(Vector2 targetPosition, Deceleration decel)
{
Vector2 ToTarget = targetPosition - this._unityGhost.Location;
float dist = ToTarget.magnitude;
if (dist > 0)
{
const float DecelerationTweaker = 0.3f;
float speed = dist / ((float)decel * DecelerationTweaker);
speed = (speed < this._unityGhost.MaxSpeed) ? speed : this._unityGhost.MaxSpeed;
Vector2 desiredVelocity = ToTarget * speed / dist;
return(desiredVelocity - this._unityGhost.Direction);
}
return(Vector2.zero);
}
Vector2 Arrive(Vector2 targetPos, Deceleration type)
{
Vector2 ToTarget = targetPos - mCurrentVehicle.GetPosition();
float dist = ToTarget.magnitude;
if (dist > 0)
{
//감속
float decelerationTweaker = 1.0f;
float speed = dist / decelerationTweaker * (float)type;
speed = Mathf.Min(speed, mCurrentVehicle.GetMaxSpeed());
Vector2 DesireVelocity = ToTarget * speed / dist;
return(DesireVelocity - mCurrentVehicle.GetVelolcity());
}
return(Vector2.zero);
}
public static Vector3 Calculate(Steering instance, Deceleration deceleration, float decelerationRate)
{
if (instance.path == null)
{
return(Vector3.zero);
}
instance.path.Update(instance.Owner.Position);
if (!instance.path.IsFinished)
{
return(Seek.Calculate(instance, instance.path.Current));
}
else
{
return(Arrive.Calculate(instance, instance.path.Current, deceleration, decelerationRate));
}
}
public static Vector3 Arrive(this Mobile vehicle, Vector3 target, Deceleration deceleration = Deceleration.normal)
{
if (vehicle == null) {
return Vector3.zero;
}
var to_target = target - vehicle.Position;
var dist = to_target.magnitude;
var steering_force = Vector3.zero;
if (dist > 0) {
const float decel_tweaker = 1.2f;
var speed = dist / ((float)deceleration * decel_tweaker);
speed = Mathf.Min(speed, vehicle.maxSpeed);
var desired_velocity = to_target * speed / dist;
steering_force = desired_velocity - vehicle.Velocity;
}
DebugExtension.DebugArrow(vehicle.Position, steering_force, Color.green);
return steering_force;
}
public static Vector3 Calculate(Steering instance, Vector3 target, Deceleration deceleration, float decelerationRate)
{
Vector3 ToTarget = target - instance.transform.position;
float dist = ToTarget.magnitude;
if (dist > 0.0)
{
float speed = dist / ((float)deceleration * decelerationRate);
//owner.maxSpeed doesnt work
speed = Mathf.Min(speed, instance.Owner.maxSpeed);
Vector3 desiredVelocity = (ToTarget * speed) / dist;
return(desiredVelocity - instance.Owner.GetVelocity());
}
return(Vector3.zero);
}
public override Vector3 Calculate(Steering instance, SteeringSettings settings)
{
pr = settings.prArrive;
weight = settings.weightArrive;
deceleration = settings.deceleration;
if (Random.Range(0.0f, 1.0f) > pr)
{
if (instance.TargetTrans != null)
{
return(Calculate(instance, instance.GetUsefulTarget(instance.TargetTrans.position), deceleration, decelerationRate) * weight / pr);
}
else
{
return(Calculate(instance, instance.GetUsefulTarget(instance.TargetPos), deceleration, decelerationRate) * weight / pr);
}
}
return(Vector3.zero);
}
public static Vector2 Arrive(MovingEntity vehicle, Deceleration deceleration)
{
//arrive = true; // NEEDS ATTENTION
float distance = Vector2.Distance(vehicle.targetPos, vehicle.position);
// savedDeceleration = (double)deceleration;
if (distance < 0.001)
return Vector2.zero;
else if (distance > 0)
{
double decelerationTweaker = 0.2;
double speed = distance / ((double)deceleration * decelerationTweaker);
speed = Mathf.Min((float)speed, vehicle.maxSpeed); // don't exceed max speed
Vector2 desiredVelocity = (vehicle.targetPos - vehicle.position) * (float)speed / distance;
return (desiredVelocity - vehicle.velocity);
}
return Vector2.zero;
}
public Vector2 Arrive(Vector2 targetPos, Deceleration deceleration)
{
Vector2 toTarget = targetPos - vehicle.Pos();
// calculate a distance to destination
// Length()(a.k.a .magnitude) is using sqrt computation. It might cause overhead.
float dist = toTarget.magnitude;
if (dist > 0)
{
const float decelerationTweaker = .3f;
float speed = dist / (float)deceleration * decelerationTweaker;
speed = Mathf.Min(speed, vehicle.GetMaxSpeed());
Vector2 desiredVelocity = toTarget * speed / dist;
Debug.Log("Arriving");
return(desiredVelocity - vehicle.GetVelocity());
}
return(new Vector2(0, 0f));
}
public Arrive(SteeringBehaviorsManager manager, Vehicle theOwner, Vector2 targetPos, Deceleration decel)
: base(manager,theOwner)
{
targetPosition = targetPos; deceleration = decel;
}
private SteerBehavior()
{
Deceleration = Deceleration.Normal;
}
protected int pathIndex; //当前的路径索引
public SteeringBehaviors(MovingEntity entity)
{
this.owner = entity;
deceleration = Deceleration.Normal;
}
public Arrive(SteeringBehaviorsManager manager, Vehicle theOwner, Vector2 targetPos, Deceleration decel)
: base(manager, theOwner)
{
targetPosition = targetPos; deceleration = decel;
}
public ArriveBehaviour(MovingEntity me, Vector2 target, Deceleration deceleration) : base(me, target)
{
this.deceleration = deceleration;
}
Vector2 Arrive(Vector2 TargetPos, Deceleration deceleration)
{
Vector2 ToTarget = TargetPos - m_Entity.Position;
//calculate the distance to the target
double dist = ToTarget.Length();
if (dist > 0)
{
//because Deceleration is enumerated as an int, this value is required
//to provide fine tweaking of the deceleration..
const double DecelerationTweaker = 0.3;
//calculate the speed required to reach the target given the desired
//deceleration
double speed = dist / ((double)deceleration * DecelerationTweaker);
//make sure the velocity does not exceed the max
speed = Math.Min(speed, m_Entity.MaxSpeed);
//from here proceed just like Seek except we don't need to normalize
//the ToTarget vector because we have already gone to the trouble
//of calculating its length: dist.
Vector2 DesiredVelocity = Vector2.Multiply(ToTarget, (float)(speed / dist));
return (DesiredVelocity - m_Entity.Velocity);
}
return Vector2.Zero;
}
private float Distance2LimitSpeed(float distance, Deceleration deceleration)
{
return(Mathf.Clamp(distance / (float)deceleration * decelerationTweaker,
_owner.MinSpeed, _owner.MaxSpeed));
}
public static string WriteDecelerationDAT(Artifact item, DateTime AbsoluteStart)
{
Deceleration decItem = item.ArtifactData as Deceleration;
StringBuilder value = new StringBuilder(255);
value.Append("EVT|");
/* 00 */
value.Append(ArtifactsHelper.GetDecelType(item).ToString(CultureInfo.InvariantCulture));
value.Append("|");
/* 01 */
value.Append(((int)((item.StartTime - AbsoluteStart).TotalSeconds * 4)).ToString(CultureInfo.InvariantCulture));
value.Append("|");
/* 02 */
value.Append(((int)((decItem.PeakTime - AbsoluteStart).TotalSeconds * 4)).ToString(CultureInfo.InvariantCulture));
value.Append("|");
/* 03 */
value.Append(((int)((item.EndTime - AbsoluteStart).TotalSeconds * 4)).ToString(CultureInfo.InvariantCulture));
value.Append("|");
/* 04 */
value.Append(string.Empty); // Y1
value.Append("|");
/* 05 */
value.Append(string.Empty); // Y2
value.Append("|");
/* 06 */
value.Append(decItem.ContractionStart.HasValue ? ((int)((decItem.ContractionStart.Value - AbsoluteStart).TotalSeconds * 4)).ToString(CultureInfo.InvariantCulture) : String.Empty);
value.Append("|");
/* 07 */
value.Append("y"); // Final
value.Append("|");
/* 08 */
value.Append(string.Empty); // Strikeout
value.Append("|");
/* 09 */
value.Append(decItem.Confidence.ToString("0.000000", CultureInfo.InvariantCulture));
value.Append("|");
/* 10 */
value.Append(decItem.Repair.ToString("0.000000", CultureInfo.InvariantCulture));
value.Append("|");
/* 11 */
value.Append(decItem.Height.ToString("0.000000", CultureInfo.InvariantCulture));
value.Append("|");
/* 12 */
value.Append(string.Empty); // Baseline variability
value.Append("|");
/* 13 */
value.Append(decItem.PeakValue.ToString("0.000000", CultureInfo.InvariantCulture));
value.Append("|");
/* 14 */
value.Append("|");
/* 15 */
if (decItem.DecelerationCategory.Equals("Variable"))
{
value.Append("y");
}
value.Append("|");
/* 16 */
value.Append("-1"); // Lag
value.Append("|");
/* 17 */
value.Append(ArtifactsHelper.GetDecelNonReassuring(item).ToString(CultureInfo.InvariantCulture));
value.Append("|");
/* 18 */
if (decItem.IsNonInterpretable)
{
value.Append("y");
}
value.Append("|");
/* 19 */
value.Append(string.Empty); // Confirmed
value.Append("|");
/* 20 */
value.Append(item.ArtifactData.Id.ToString(CultureInfo.InvariantCulture));
return(value.ToString());
}
public OwnArriveBehaviour(AgentBase agent, Vector3 target, Deceleration deceleration) : base(agent)
{
_target = target;
_deceleration = deceleration;
}
public Vector3 Arrive(Vector3 targetPosition, Deceleration deceleration)
{
Vector3 toTarget = targetPosition - vehicle.Position;
float distToTarget = toTarget.magnitude;
if(distToTarget > 0)
{
const float decelerationTweaker = 0.3f;
float speed = distToTarget / ((float)deceleration * decelerationTweaker);
Mathf.Min(speed, vehicle.maxSpeed);
Vector3 desiredV = toTarget * (speed / distToTarget);
return desiredV - vehicle.Velocity;
}
return Vector3.zero;
}
public void Set(Vec3 targetPos, Deceleration deceleration)
{
this._targetPos = targetPos;
this._deceleration = deceleration;
this.complete = false;
}
public SteeringBehavior(MovingEntity agent)
{
m_parentMovingEntity = agent;
m_iFlags = 0;
m_dDBoxLength = SteerParams.Instance.MinDetectionBoxLength;
m_dWeightCohesion = SteerParams.Instance.AppliedCohesionWeight();
m_dWeightAlignment = SteerParams.Instance.AppliedAlignmentWeight();
m_dWeightSeparation = SteerParams.Instance.AppliedSeparationWeight();
m_dWeightObstacleAvoidance = SteerParams.Instance.AppliedObstacleAvoidanceWeight();
m_dWeightWander = SteerParams.Instance.AppliedWanderWeight();
m_dWeightWallAvoidance = SteerParams.Instance.AppliedWallAvoidanceWeight();
m_dViewDistance = SteerParams.Instance.ViewDistance;
m_dWallDetectionFeelerLength = SteerParams.Instance.WallDetectionFeelerLength;
m_Feelers = new List<Vector2D>(3);
m_Deceleration = Deceleration.normal;
m_pTargetAgent1 = null;
m_pTargetAgent2 = null;
m_dWanderDistance = WanderDist;
m_dWanderJitter = WanderJitterPerSec;
m_dWanderRadius = WanderRad;
m_dWaypointSeekDistSq = WaypointSeekDist * WaypointSeekDist;
m_dWeightSeek = SteerParams.Instance.AppliedSeekWeight();
m_dWeightFlee = SteerParams.Instance.AppliedFleeWeight();
m_dWeightArrive = SteerParams.Instance.AppliedArriveWeight();
m_dWeightPursuit = SteerParams.Instance.AppliedPursuitWeight();
m_dWeightOffsetPursuit = SteerParams.Instance.AppliedOffsetPursuitWeight();
m_dWeightInterpose = SteerParams.Instance.AppliedInterposeWeight();
m_dWeightHide = SteerParams.Instance.AppliedHideWeight();
m_dWeightEvade = SteerParams.Instance.AppliedEvadeWeight();
m_dWeightFollowPath = SteerParams.Instance.AppliedFollowPathWeight();
m_SummingMethod = summing_method.prioritized;
//stuff for the wander behavior
double theta = Utils.RandFloat() * Utils.TwoPi;
//create a vector to a target position on the wander circle
m_vWanderTarget = new Vector2D(m_dWanderRadius * Math.Cos(theta), m_dWanderRadius * Math.Sin(theta));
//create a Path
m_pPath = new Path2D(true);
m_vSteeringForce = new Vector2D();
}
public ArriveBehaviour(MovingEntity me, Deceleration d) : base(me)
{
this.me = me;
this.decel = d;
}
