public static double DeltaAngle(double current, double target)
{
double delta = WMath.Repeat((target - current), 360.0F);
if (delta > 180.0F)
{
delta -= 360.0F;
}
return(delta);
}
public Sweep SweepCollide(AABBCollider colliding, Vector3D collidingVelocity, AABBCollider collider)
{
AABB box = colliding.AABB;
AABB col = collider.AABB;
Vector3D delta = collidingVelocity;
Sweep sweep = new Sweep();
if (delta == Vector3D.Zero)
{
sweep.Position = box.Position;
sweep.Hit = this.Collide(colliding, collider);
if (sweep.Hit.HasHit)
{
sweep.Hit.Time = 0.0D;
sweep.Time = 0.0D;
}
else
{
sweep.Time = 1.0;
}
return(sweep);
}
sweep.Hit = new RayBoxCollisionProvider().Collide(new Ray(box.Position, delta, delta.Length), collider, box.Extents);
if (sweep.Hit.HasHit)
{
sweep.Time = WMath.Clamp(sweep.Hit.Time - Physics.Epsilon, 0, 1);
sweep.Position = box.Position + delta * sweep.Time;
Vector3D direction = delta.Normalized;
sweep.Hit.Position = new Vector3D(
WMath.Clamp(sweep.Hit.Position.X + direction.X * box.Extents.X, col.Position.X - col.Extents.X,
col.Position.X + col.Extents.X),
WMath.Clamp(sweep.Hit.Position.Y + direction.Y * box.Extents.Y, col.Position.Y - col.Extents.Y,
col.Position.Y + col.Extents.Y),
WMath.Clamp(sweep.Hit.Position.Z + direction.Z * box.Extents.Z, col.Position.Z - col.Extents.Z,
col.Position.Z + col.Extents.Z)
);
}
else
{
sweep.Position = box.Position + delta;
sweep.Time = 1.0D;
}
return(sweep);
}
public Hit Collide(Ray colliding, AABBCollider collider, Vector3D padding)
{
Vector3D pos = colliding.Origin;
Vector3D delta = colliding.Direction * colliding.Length;
AABB aabb = collider.AABB;
Vector3D scale = new Vector3D(1.0D / delta.X, 1.0 / delta.Y, 1.0 / delta.Z);
int signX = Math.Sign(scale.X);
int signY = Math.Sign(scale.Y);
int signZ = Math.Sign(scale.Z);
double nearTimeX = (aabb.Position.X - signX * (aabb.Extents.X + padding.X) - pos.X) * scale.X;
double nearTimeY = (aabb.Position.Y - signY * (aabb.Extents.Y + padding.Y) - pos.Y) * scale.Y;
double nearTimeZ = (aabb.Position.Z - signZ * (aabb.Extents.Z + padding.Z) - pos.Z) * scale.Z;
double farTimeX = (aabb.Position.X + signX * (aabb.Extents.X + padding.X) - pos.X) * scale.X;
double farTimeY = (aabb.Position.Y + signY * (aabb.Extents.Y + padding.Y) - pos.Y) * scale.Y;
double farTimeZ = (aabb.Position.Z + signZ * (aabb.Extents.Z + padding.Z) - pos.Z) * scale.Z;
if (nearTimeX > farTimeY || nearTimeX > farTimeZ ||
nearTimeY > farTimeX || nearTimeY > farTimeZ ||
nearTimeZ > farTimeX || nearTimeZ > farTimeY)
{
return(new Hit());
}
double nearTime = Math.Max(Math.Max(nearTimeX, nearTimeY), nearTimeZ);
double farTime = Math.Max(Math.Max(farTimeX, farTimeY), farTimeZ);
if (nearTime >= 1 || farTime <= 0)
{
return(new Hit());
}
Hit hit = new Hit(collider)
{
Time = WMath.Clamp(nearTime, 0, 1)
};
Vector3D n = Vector3D.Zero;
hit.Delta = -delta * (1.0 - hit.Time);
hit.Position = pos + delta * hit.Time;
hit.Normal = GetNormalFromPoint(collider, hit.Position);
return(hit);
}
public Vector2D NextDirection2D()
{
double rad = WMath.Remap(NextDouble(), 0, 1, -Math.PI, Math.PI);
return(new Vector2D(Math.Cos(rad), Math.Sin(rad)).Normalized);
}