public static Vector3d SmoothDamp(Vector3d current, Vector3d target, ref Vector3d currentVelocity,
double smoothTime, double maxSpeed, double deltaTime)
{
smoothTime = CGMath.Max(0.0001d, smoothTime);
double num1 = 2d / smoothTime;
double num2 = num1 * deltaTime;
double num3 = (1.0d / (1.0d + num2 + 0.479999989271164d * num2 * num2 +
0.234999999403954d * num2 * num2 * num2));
Vector3d vector = current - target;
Vector3d vector3_1 = target;
double maxLength = maxSpeed * smoothTime;
Vector3d vector3_2 = Vector3d.ClampMagnitude(vector, maxLength);
target = current - vector3_2;
Vector3d vector3_3 = (currentVelocity + num1 * vector3_2) * deltaTime;
currentVelocity = (currentVelocity - num1 * vector3_3) * num3;
Vector3d vector3_4 = target + (vector3_2 + vector3_3) * num3;
if (!(Vector3d.Dot(vector3_1 - current, vector3_4 - vector3_1) > 0.0))
{
return(vector3_4);
}
vector3_4 = vector3_1;
currentVelocity = (vector3_4 - vector3_1) / deltaTime;
return(vector3_4);
}
public void ClampMagnitude()
{
for (int i = 0; i < count; i++)
{
float ax, ay, az;
float p;
ax = UnityEngine.Random.Range(-10F, 10F);
ay = UnityEngine.Random.Range(-10F, 10F);
az = UnityEngine.Random.Range(-10F, 10F);
p = UnityEngine.Random.Range(-20F, 20F);
Vector3 a = new Vector3(ax, ay, az);
Vector3d ad = new Vector3d(ax, ay, az);
Vector3 value = Vector3.ClampMagnitude(a, p);
Vector3d valued = Vector3d.ClampMagnitude(ad, p);
Assert.True(Approximate(value, valued));
}
}
public override void GetDesiredSteering(SteeringResult rst)
{
_selfCollisionPos = Vector3d.zero;
_neighbors.Clear();
var bs = LogicCore.SP.SceneManager.CurrentScene as BattleScene;
// bs.FixedQuadTreeForBuilding.Query(Self, FixedMath.One*6, _neighbors);
// LogicCore.SP.SceneManager.CurrentScene.FixedQuadTree.Query(Self, FixedMath.One * 4, _neighbors);
if (_neighbors.Count == 0)
{
return;
}
var avoidVec = Avoid(_neighbors, _neighbors.Count, Self.Velocity);
if (avoidVec.sqrMagnitude < this.MinimumAvoidVectorMagnitude.Mul(MinimumAvoidVectorMagnitude))
{
return;
}
avoidVec =
Vector3d.ClampMagnitude(avoidVec.Div(LockFrameMgr.FixedFrameTime), Self.MaxDeceleration);
rst.DesiredSteering = avoidVec;
}
protected override void Respond(DRigidbody drb)
{
if (Vector3d.SqrMagnitude(position - drb.position) < radius * radius)
{
Vector3d contactPosition = Vector3d.ClampMagnitude(drb.position - position, radius) + position;
drb.position = contactPosition;
drb.AddForce(new Vector3d(0, 9.81, 0));
Vector3d diff = drb.position - position;
Vector3d normal = diff.normalized;
Debug.DrawRay((Vector3)contactPosition, (Vector3)normal);
if (Vector3d.Dot(normal, drb.velocity) < 0)
{
drb.velocity = Vector3d.ProjectOnPlane(drb.velocity, normal);
}
}
}