// ----------------------------------------------------------------------------
// measure path curvature (1/turning-radius), maintain smoothed version
void measurePathCurvature(float elapsedTime)
{
if (elapsedTime > 0)
{
Vector3 pos = Position;
Vector3 fwd = Forward;
Vector3 dP = _lastPosition - pos;
Vector3 dF = (_lastForward - fwd) / dP.magnitude;
//SI - BIT OF A WEIRD FIX HERE . NOT SURE IF ITS CORRECT
//Vector3 lateral = dF.perpendicularComponent (forward ());
Vector3 lateral = OpenSteerUtility.perpendicularComponent(dF, fwd);
float sign = (Vector3.Dot(lateral, Side) < 0) ? 1.0f : -1.0f;
_curvature = lateral.magnitude * sign;
/*
* If elapsedTime is greater than 0.25, that means that blendIntoAccumulator
* will end up clipping the first parameter to [0..1], and we'll lose information,
* making this call framerate-dependent.
*
* No idea where that 4.0f value comes from, probably out of a hat.
*/
_smoothedCurvature = OpenSteerUtility.blendIntoAccumulator(elapsedTime * 4.0f,
_curvature,
_smoothedCurvature);
_lastForward = fwd;
_lastPosition = pos;
}
}
public static Vector3 vecLimitDeviationAngleUtility(bool insideOrOutside, Vector3 source, float cosineOfConeAngle, Vector3 basis)
{
float magnitude = source.get_magnitude();
if (magnitude == 0f)
{
return(source);
}
Vector3 vector = source / magnitude;
float num = Vector3.Dot(vector, basis);
if (insideOrOutside)
{
if (num >= cosineOfConeAngle)
{
return(source);
}
}
else if (num <= cosineOfConeAngle)
{
return(source);
}
Vector3 vector2 = OpenSteerUtility.perpendicularComponent(source, basis);
float num2 = (float)Math.Sqrt((double)(1f - cosineOfConeAngle * cosineOfConeAngle));
Vector3 vector3 = basis * cosineOfConeAngle;
Vector3 vector4 = vector2.get_normalized() * num2;
return((vector3 + vector4) * magnitude);
}
// ----------------------------------------------------------------------------
// avoidance of "close neighbors" -- used only by steerToAvoidNeighbors
//
// XXX Does a hard steer away from any other agent who comes withing a
// XXX critical distance. Ideally this should be replaced with a call
// XXX to steerForSeparation.
public Vector3 steerToAvoidCloseNeighbors(float minSeparationDistance, ArrayList others)
{
// for each of the other vehicles...
Vector3 result = Vector3.zero;
for (int i = 0; i < others.Count; i++)
{
SteeringVehicle other = (SteeringVehicle)others[i];
if (other != this)
{
float sumOfRadii = Radius + other.Radius;
float minCenterToCenter = minSeparationDistance + sumOfRadii;
Vector3 offset = other.Position - Position;
float currentDistance = offset.magnitude;
if (currentDistance < minCenterToCenter)
{
result = OpenSteerUtility.perpendicularComponent(-offset, Forward);
#if ANNOTATE_AVOIDNEIGHBORS
annotateAvoidCloseNeighbor(other, result);
#endif
}
}
}
return(result);;
}
private void measurePathCurvature(float elapsedTime)
{
if (elapsedTime > 0f)
{
Vector3 position = base.Position;
Vector3 forward = base.Forward;
Vector3 vector = this._lastPosition - position;
Vector3 source = (this._lastForward - forward) / vector.get_magnitude();
Vector3 vector2 = OpenSteerUtility.perpendicularComponent(source, forward);
float num = (Vector3.Dot(vector2, base.Side) >= 0f) ? -1f : 1f;
this._curvature = vector2.get_magnitude() * num;
this._smoothedCurvature = OpenSteerUtility.blendIntoAccumulator(elapsedTime * 4f, this._curvature, this._smoothedCurvature);
this._lastForward = forward;
this._lastPosition = position;
}
}
public Vector3 steerToAvoidCloseNeighbors(float minSeparationDistance, ArrayList others)
{
Vector3 vector = Vector3.get_zero();
for (int i = 0; i < others.get_Count(); i++)
{
SteeringVehicle steeringVehicle = (SteeringVehicle)others.get_Item(i);
if (steeringVehicle != this)
{
float num = base.Radius + steeringVehicle.Radius;
float num2 = minSeparationDistance + num;
Vector3 vector2 = steeringVehicle.Position - base.Position;
float magnitude = vector2.get_magnitude();
if (magnitude < num2)
{
vector = OpenSteerUtility.perpendicularComponent(-vector2, base.Forward);
this.annotateAvoidCloseNeighbor(steeringVehicle, vector);
}
}
}
return(vector);
}