public override void Update(State s, Room room)
{
base.Update(s, room);
Vector2 direction = new Vector2((float)((TargetX - x) * (DiverGame.Random.NextDouble() * 0.2f + 0.8f)),
(float)((TargetY - y) * (DiverGame.Random.NextDouble() * 0.2f + 0.8f)));
if(direction.LengthSquared() > 0)
direction.Normalize();
speedX += direction.X * 0.007f;
speedY += direction.Y * 0.007f;
speedX *= 0.999f;
speedY *= 0.999f;
float speed = (float)Math.Sqrt(speedX * speedX + speedY * speedY);
animationGridFrame += speed * 0.25f + 0.03f;
x += speedX;
y += speedY;
X = (int)x;
Y = (int)y;
float desiredRot = (float)Math.Atan2(speedX, -speedY) - (float)Math.PI / 2f;
float rotDiff = desiredRot - rotation;
while (rotDiff > MathHelper.Pi) rotDiff -= MathHelper.TwoPi;
while (rotDiff < -MathHelper.Pi) rotDiff += MathHelper.TwoPi;
rotation += rotDiff * 0.1f;
}
public void Draw(SpriteBatch spriteBatch, float playerForwardRadians, Vector3 enemyPos, ref Vector3 playerPos)
{
// The last parameter of the color determines how transparent the radar circle will be
spriteBatch.Draw(RadarImage, RadarCenterPos, null, new Color(100, 100, 100, 150), 0.0f, RadarImageCenter, RadarScreenRadius / (RadarImage.Height * 0.5f), SpriteEffects.None, 0.0f);
// If enemy is in range
Vector2 diffVect = new Vector2(enemyPos.X - playerPos.X, enemyPos.Z - playerPos.Z);
float distance = diffVect.LengthSquared();
// Check if enemy is within RadarRange
if (distance < RadarRangeSquared)
{
// Scale the distance from world coords to radar coords
diffVect *= RadarScreenRadius / RadarRange;
// We rotate each point on the radar so that the player is always facing UP on the radar
diffVect = Vector2.Transform(diffVect, Matrix.CreateRotationZ(playerForwardRadians));
// Offset coords from radar's center
diffVect += RadarCenterPos;
// We scale each dot so that enemies that are at higher elevations have bigger dots, and enemies
// at lower elevations have smaller dots.
float scaleHeight = 1.0f - ((enemyPos.Y - playerPos.Y) / 200000.0f);
// Draw enemy dot on radar
spriteBatch.Draw(EnemyDotImage, diffVect, null, Color.White, 0.0f, new Vector2(0.0f, 0.0f), scaleHeight, SpriteEffects.None, 0.0f);
}
// Draw player's dot last
spriteBatch.Draw(PlayerDotImage, RadarCenterPos, Color.White);
}
public static void correctOverlap(Agent agent, List<Flockable> neighbors)
{
double distSq;
double overlap;
double newX, newY;
foreach (Flockable other in neighbors)
{
if (other is Agent == true)
{
Vector2 toOther = new Vector2((float)(other.getLocation().X - agent.getLocation().X), (float)(other.getLocation().Y - agent.getLocation().Y));
distSq = toOther.LengthSquared();
//distSq = (Math.Pow((agent.getLocation().X - other.getLocation().X), 2) + Math.Pow((agent.getLocation().Y - other.getLocation().Y), 2));
overlap = agent.getAgentRadiusSq() + other.getAgentRadiusSq() - distSq;
if (agent.Equals(other) == false && overlap >= 0)
{
double scale = overlap / Math.Sqrt(distSq);
Vector2 move = new Vector2((float)(toOther.X * scale), (float)(toOther.Y * scale));
//newX = agent.getLocation().X + (((other.getLocation().X - agent.getLocation().X) / Math.Sqrt(distSq)) * overlap);
//newY = agent.getLocation().Y + (((other.getLocation().Y - agent.getLocation().Y) / Math.Sqrt(distSq)) * overlap);
newX = agent.getLocation().X + move.X;
newY = agent.getLocation().Y + move.Y;
agent.setLocation(new DotNET.Point(newX, newY));
}
}
} // end foreach
}
public override GraphicsToolkit.Physics._3D.Contact3D GenerateContact(RigidBody3D rb, float dt)
{
if (rb as SphereBody != null)
{
SphereBody c = rb as SphereBody; //sphere that this body is colliding with
Vector3 pa = c.Pos; //point on this body closest to the sphere
Vector2 XYVec = new Vector2(pa.X - 0.5f, pa.Y - 0.5f);
if (XYVec.LengthSquared() < 1) //if inside bounding box but outside actual block
{
pa.X = ((pa.X - 0.5f) / XYVec.Length()) + 0.5f;
pa.Y = ((pa.Y - 0.5f) / XYVec.Length()) + 0.5f;
}
pa.X = MathHelper.Clamp(pa.X, -0.5f, 0.5f);
pa.Y = MathHelper.Clamp(pa.Y, -0.5f, 0.5f);
pa.Z = MathHelper.Clamp(pa.Z, -0.5f, 0.5f);
Vector3 normal = rb.Pos - pa;
float normLen = normal.Length();
float dist = normLen - c.Radius; //distance from block to sphere
normal /= normLen; //normalize normal
Vector3 pb = rb.Pos - normal * c.Radius; //closest point on sphere
return new Contact3D(normal, dist, this, rb, pa, pb);
}
else
{
throw new NotImplementedException();
}
}
//Guess what I stole from StackOverflow
public Texture2D createCircleText(GraphicsDevice graphics, int radius, Color circleColor)
{
Texture2D texture = new Texture2D(graphics, radius, radius);
Color[] colorData = new Color[radius * radius];
float diam = radius / 2f;
float diamsq = diam * diam;
for (int x = 0; x < radius; x++)
{
for (int y = 0; y < radius; y++)
{
int index = x * radius + y;
Vector2 pos = new Vector2(x - diam, y - diam);
if (pos.LengthSquared() <= diamsq)
{
colorData[index] = circleColor;
}
else
{
colorData[index] = Color.Transparent;
}
}
}
texture.SetData(colorData);
return texture;
}
// Methods
public static void init(GraphicsDevice graphDev, SpriteBatch spriteBatch)
{
// init pointTexture
pointTexture = new Texture2D(graphDev, 1, 1, false, SurfaceFormat.Color);
pointTexture.SetData<Color>(new Color[] { defaultColor });
// init circleTexture
circleTexture = new Texture2D(graphDev, circleRadius, circleRadius);
Color[] colorData = new Color[circleRadius * circleRadius];
float diam = circleRadius / 2f;
float diamsq = diam * diam;
for (int x = 0; x < circleRadius; x++)
{
for (int y = 0; y < circleRadius; y++)
{
int index = x * circleRadius + y;
Vector2 pos = new Vector2(x - diam, y - diam);
if (pos.LengthSquared() <= diamsq)
{
colorData[index] = defaultColor;
}
else
{
colorData[index] = Color.Transparent;
}
}
}
circleTexture.SetData(colorData);
// spriteBatch reference
sb = spriteBatch;
}
static public Texture2D createCircle(GraphicsDevice graphics, int durchmesser)
{
if (durchmesser == 0)
durchmesser = 1;
Texture2D texture = new Texture2D(graphics, durchmesser, durchmesser);
Color[] colorData = new Color[durchmesser * durchmesser];
float diam = durchmesser / 2;
float diamsq = diam * diam;
for (int x = 0; x < durchmesser; x++)
{
for (int y = 0; y < durchmesser; y++)
{
int index = x * durchmesser + y;
Vector2 pos = new Vector2(x - diam, y - diam);
if (pos.LengthSquared() <= diamsq)
{
colorData[index] = Color.White;
}
else
{
colorData[index] = Color.Transparent;
}
}
}
texture.SetData(colorData);
return texture;
}
public static float GetSweptAngle(Vector2 start, Vector2 stop)
{
float lengthSquared = start.LengthSquared();
if (lengthSquared == 0.0f)
throw new ArgumentException("start");
if (Math.Abs(lengthSquared - 1.0f) > 0.00005)
start.Normalize();
lengthSquared = stop.LengthSquared();
if (lengthSquared == 0.0f)
throw new ArgumentException("stop");
if (Math.Abs(lengthSquared - 1.0f) > 0.00005)
stop.Normalize();
float dot = Vector2.Dot(start, stop);
Vector3 start3D = new Vector3(start, 0.0f);
Vector3 stop3D = new Vector3(stop, 0.0f);
Vector3 cross = Vector3.Cross(start3D, stop3D);
float theta;
if (cross.Z == 0.0f)
{
if (dot < 0.0f)
return MathHelper.Pi;
else
return 0.0f;
}
theta = (float)(Math.Acos(MathHelper.Clamp(dot, -1.0f, 1.0f)));
return Math.Sign(cross.Z) == 1 ? theta : -theta;
}
public Vector3 intersects(CollisionCylinder other)
{
float yDiff = this.position.Y - other.position.Y;
float combinedHeight = this.Height + other.Height;
float combinedRadius = this.Radius + other.Radius;
//first check if height intersects
if (Math.Abs(yDiff) < combinedHeight )
{
Vector2 xzDiff = new Vector2(this.position.X - other.position.X,this.position.Z - other.position.Z);
if (xzDiff.LengthSquared() < combinedRadius * combinedRadius)
{
float intersectY = combinedHeight - yDiff;
Vector2 intersectXZ = Vector2.Normalize(xzDiff) * (combinedRadius - xzDiff.Length());
if (intersectY * intersectY < intersectXZ.LengthSquared())
{
return Vector3.Up * (combinedHeight - yDiff);
}
else
{
return new Vector3(intersectXZ.X,0f, intersectXZ.Y);
}
}
}
return Vector3.Zero;
}
public static Shape GetCircle(GraphicsDevice device, int radius)
{
Shape texture = new Shape(device, radius, radius, EShapes.Circle);
Color[] colorData = new Color[radius * radius];
float diam = radius / 2f;
float diamsq = diam * diam;
for (int x = 0; x < radius; x++)
{
for (int y = 0; y < radius; y++)
{
int index = x * radius + y;
Vector2 pos = new Vector2(x - diam, y - diam);
if (pos.LengthSquared() <= diamsq)
{
colorData[index] = Color.White;
}
else
{
colorData[index] = Color.Transparent;
}
}
}
texture.SetData(colorData);
return texture;
}
public void Walk(Vector2 direction)
{
if(direction.LengthSquared() > 0)
{
direction.Normalize();
body.Move(_walkSpeed * direction);
}
}
public static void ProjectVector2D(ref Vector2 vec, ref Vector2 projectOn, out Vector2 result)
{
float dp;
Vector2.Dot(ref vec, ref projectOn, out dp);
float oneOnLenSqr = 1.0f / projectOn.LengthSquared();
result = new Vector2(
dp * oneOnLenSqr * projectOn.X,
dp * oneOnLenSqr * projectOn.Y);
}
/// <summary>
/// Determines if a circle contains a Point
/// </summary>
/// <returns>True if the circle and Point overlap. False otherwise.</returns>
public bool Contains(Point p)
{
this.v = new Vector2(p.X,p.Y);
this.direction = Center - v;
this.distanceSquared = direction.LengthSquared();
return ((distanceSquared > 0) && (distanceSquared < Radius * Radius));
}
/// <summary>
/// Determines if a circle intersects a rectangle.
/// </summary>
/// <returns>True if the circle and rectangle overlap. False otherwise.</returns>
public bool Intersects(Rectangle rectangle)
{
this.v = new Vector2(MathHelper.Clamp(Center.X, rectangle.Left, rectangle.Right),
MathHelper.Clamp(Center.Y, rectangle.Top, rectangle.Bottom));
this.direction = Center - v;
this.distanceSquared = direction.LengthSquared();
return ((distanceSquared > 0) && (distanceSquared < Radius * Radius));
}
public void DrawDot(SpriteBatch spriteBatch, Vector2 locationDelta, Color color)
{
if (locationDelta.LengthSquared() < detectionRadius * detectionRadius)
{
spriteBatch.Draw(dotTexture,
new Rectangle(
rect.Center.X + (int)(locationDelta.X * 0.88f * textureRadius / detectionRadius),
rect.Center.Y + (int)(locationDelta.Y * 0.88f * textureRadius / detectionRadius),
(int)(rect.Width * 0.05f), (int)(rect.Height * 0.05f)), color);
}
}
public void Shake(Vector2 magnitude, float shakeTime)
{
if (magnitude.LengthSquared() < shakeTarget.LengthSquared() || magnitude.Length() < 0.01f)
{
resetScreenShake();
return;
}
shakeTimer = 0.0f;
shakePosition = magnitude;
shakeTarget = -shakePosition * shakeCooldown;
this.shakeTime = shakeTime;
}
public override void Collide(GameNode node)
{
for (int i = Enemy.Enemies.Count - 1; i >= 0; i--)
{
line = this.Position - Enemy.Enemies[i].Position;
if (line.LengthSquared() < (40000))
{
Enemy.Enemies[i].TakeDamage(this.Damage, this);
}
}
base.Collide(node);
}
public void SetVelocity(Vector2 velocity)
{
if (velocity.LengthSquared() <= (m_maxVelocity * m_maxVelocity))
{
this.m_Velocity = velocity;
}
else
{
Vector2 unitVelocity = velocity;
unitVelocity.Normalize();
this.m_Velocity = m_maxVelocity * unitVelocity;
}
}
/// <summary>Determines the contact location between a line and a disc</summary>
/// <param name="lineOffset">
/// Offset of the line relative to the disc's center
/// </param>
/// <param name="lineDirection">Direction and length of the line</param>
/// <param name="discRadius">Radius of the disc</param>
/// <returns>The point of intersection of the line with the disc, if any</returns>
/// <remarks>
/// <para>
/// Shamelessly lifted from the FreeMagic library at http://www.magic-software.com
/// and used as a supporting function for the other line/sphere contact finders.
/// </para>
/// </remarks>
internal static LineContacts FindContacts(
Vector2 lineOffset, Vector2 lineDirection, float discRadius
) {
float a0 = lineOffset.LengthSquared() - discRadius * discRadius;
float a1 = Vector2.Dot(lineDirection, lineOffset);
float discrete = a1 * a1 - a0;
if(discrete > 0.0f) {
discrete = (float)Math.Sqrt(discrete);
return new LineContacts(-a1 - discrete, -a1 + discrete);
} else {
return LineContacts.None;
}
}
public static bool IsProjective(Vector2 point, Vector2 p0, Vector2 p1)
{
Vector2 p1p0 = new Vector2(p1.X -p0.X, p1.Y - p0.Y);
Vector2 p0p1 = Vector2.Negate(p1p0);
Vector2 p1R = new Vector2(p1.X - point.X, p1.Y - point.Y);
Vector2 p0R = new Vector2(p0.X - point.X, p0.Y - point.Y);
double A = p1p0.LengthSquared();
double B = p1R.LengthSquared();
double C = p0R.LengthSquared();
if (B > (A + C)) return false;
if (C > (A + B)) return false;
return true;
}
/// <summary>
/// Determines if a circle intersects a rectangle.
/// </summary>
/// <returns>True if the circle and rectangle overlap. False otherwise.</returns>
public bool Intersects(Rectangle rectangle)
{
this.v = new Vector2(MathHelper.Clamp(Center.X, rectangle.Left, rectangle.Right),
MathHelper.Clamp(Center.Y, rectangle.Top, rectangle.Bottom));
if (v == this.Center)
return true;
this.direction = Center - v;
this.distanceSquared = direction.LengthSquared();
//Console.WriteLine("Distance: " + this.distanceSquared.ToString());
return ((distanceSquared > 0) && (distanceSquared < (Radius * Radius)));
}
public void DrawCircle(Vector2 midpoint, float radius, float thickness, Color color)
{
float radsq = (float)Math.Pow(radius, 2);
Vector2 current = new Vector2(radius, 0);
Vector2 currentInv = new Vector2(radius, 0);
for (; current.X >= 0; --current.X, --currentInv.X) {
while (current.LengthSquared() < radsq) {
sb.Draw(pixel, midpoint + current, color);
sb.Draw(pixel, midpoint - current, color);
sb.Draw(pixel, midpoint + currentInv, color);
sb.Draw(pixel, midpoint - currentInv, color);
--current.Y;
++currentInv.Y;
}
++current.Y;
--currentInv.Y;
}
}
public static Vector2 GetHoriztontalMovementDirection()
{
Vector2 direction = new Vector2();
if (keyboardState.IsKeyDown(Keys.A))
direction.X -= 1;
if (keyboardState.IsKeyDown(Keys.D))
direction.X += 1;
if (keyboardState.IsKeyDown(Keys.W))
direction.Y -= 1;
if (keyboardState.IsKeyDown(Keys.S))
direction.Y += 1;
// Clamp the length of the vector to a maximum of 1.
if (direction.LengthSquared() > 1)
direction.Normalize();
return direction;
}
private static void CalculateGradients(out Vector2[] grad)
{
grad = new Vector2[256];
for (var i = 0; i < grad.Length; i++)
{
Vector2 gradient;
do
{
gradient = new Vector2((float)(_random.NextDouble() * 2 - 1), (float)(_random.NextDouble() * 2 - 1));
}
while (gradient.LengthSquared() >= 1);
gradient.Normalize();
grad[i] = gradient;
}
}
public void Update(float dt)
{
if(m_GM.Player == null)
return;
Vector3 playerPos = m_GM.Player.Owner.GetWorldTranslation();
Vector3 current = Owner.GetWorldTranslation();
Vector3 toplayer3D = playerPos - current;
Vector2 toplayer = new Vector2(toplayer3D.X, toplayer3D.Y);
if (toplayer.LengthSquared() < m_GM.InfluenceRadius*m_GM.InfluenceRadius)
{
Target = m_GM.Player.Owner;
}
else
{
Target = null;
}
}
public static Texture2D Circle(float radius, Color color = new Color())
{
int rad = (int)radius;
int diam = rad * 2;
Texture2D texture = new Texture2D(graphics.GraphicsDevice, diam, diam);
Color[] colorData = new Color[diam * diam];
float radsq = radius * radius;
for (int x = 0; x < diam; x++) {
for (int y = 0; y < diam; y++) {
int i = x * diam + y;
Vector2 pos = new Vector2(x - rad, y - rad);
if (pos.LengthSquared() <= radsq) {
colorData[i] = color;
}
else {
colorData[i] = Color.Transparent;
}
}
}
texture.SetData<Color>(colorData);
return texture;
}
public Vector2 computeAlignment(NPC member)
{
Vector2 v = new Vector2();
int neighbourCount = 0;
foreach (NPC npc in Members)
{
if (npc != member)
{
float distSq = (npc.Position - member.Position).LengthSquared();
if (distSq < flockDist * flockDist)
{
v += npc.ComponentPhysics.Velocity;
++neighbourCount;
}
}
}
if (neighbourCount == 0 || v.LengthSquared() == 0)
return v;
v /= neighbourCount;
v.Normalize();
return v * alignmentWeight;
}
private Microsoft.Xna.Framework.Vector2 LineSegmentPointNearestOrigin(Microsoft.Xna.Framework.Vector2 start, Microsoft.Xna.Framework.Vector2 end)
{
var delta = end - start;
var perp = new Vector2(delta.Y, -delta.X);
if (delta.LengthSquared() < Tolerance)
{
return(start);
}
var intersection = LineAlgorithms.LineSegmentIntersection(
start, end, Vector2.Zero, perp);
if (intersection.WithinFirstSegment)
{
return(intersection.IntersectionPoint);
}
return(start.LengthSquared() < end.LengthSquared()
? start
: end);
}
public void Update(GameTime gameTime, Vector3 look)
{
//transform the 3d vectors to 2d vectors
playerLook = new Vector2(-look.X, -look.Z);
differanceVect = new Vector2(Game1.getInstance().getEnemy().getPos().X - Game1.getInstance().getPlayer().getPos().X, Game1.getInstance().getEnemy().getPos().Z - Game1.getInstance().getPlayer().getPos().Z);
float distance = differanceVect.LengthSquared();
if (distance < radarRangeSquared)
{
// Scale the distance from world coords to radar coords
differanceVect *= radarScreenRadius / radarRange;
// We rotate each point on the radar so that the player is always facing UP on the radar
differanceVect = Vector2.Transform(differanceVect, Matrix.CreateRotationZ(MathHelper.ToRadians(0)));
// Offset coords from radar's center
differanceVect += radarCenterPos;
playerPos = new Vector2(Game1.getInstance().getPlayer().getPos().X, Game1.getInstance().getPlayer().getPos().Z);
playerPos *= radarScreenRadius / radarRange;
playerPos += radarCenterPos;
}
rotation = -(float)Math.Atan2(playerLook.X, playerLook.Y);
base.Update(gameTime);
}
public virtual void attemptMove(Vector2 movement)
{
if (!alive) return;
if (curState != CharacterState.Standing && curState != CharacterState.Walking) return;
if (movement.LengthSquared() > .05)
{
float nextX = position.X + (movement.X * moveSpeed);
float nextY = position.Y + (movement.Y * moveSpeed);
if (!canMove(new Vector2(movement.X * moveSpeed, 0)))
{
nextX = position.X;
}
if (!canMove(new Vector2(0, movement.Y * moveSpeed)))
{
nextY = position.Y;
}
position = new Vector2(nextX, nextY);
setCharacterState(CharacterState.Walking);
if (Math.Abs(movement.X) > Math.Abs(movement.Y))
{
if (movement.X > 0) curDirection = Direction.Right;
if (movement.X < 0) curDirection = Direction.Left;
}
if (Math.Abs(movement.Y) > Math.Abs(movement.X))
{
if (movement.Y > 0) curDirection = Direction.Forward;
if (movement.Y < 0) curDirection = Direction.Back;
}
}
else setCharacterState(CharacterState.Standing);
}
public void DrawCircleFilled(Vector2 midpoint, float radius, float thickness, Color outline, Color fill)
{
float radsq = (float)Math.Pow(radius, 2);
Vector2 current = new Vector2(radius, 0);
Vector2 currentInv = new Vector2(radius, 0);
for (; current.X > 0; --current.X, --currentInv.X) {
DrawLine(midpoint + current, midpoint + currentInv, 1.0f, fill);
DrawLine(midpoint - current, midpoint - currentInv, 1.0f, fill);
while (current.LengthSquared() < radsq) {
sb.Draw(pixel, midpoint + current, outline);
sb.Draw(pixel, midpoint - current, outline);
sb.Draw(pixel, midpoint + currentInv, outline);
sb.Draw(pixel, midpoint - currentInv, outline);
--current.Y;
++currentInv.Y;
}
++current.Y;
--currentInv.Y;
}
sb.Draw(pixel, midpoint + current, outline);
sb.Draw(pixel, midpoint - current, outline);
sb.Draw(pixel, midpoint + currentInv, outline);
sb.Draw(pixel, midpoint - currentInv, outline);
}
public MinkowskiSimplex FindMinkowskiSimplex(Microsoft.Xna.Framework.Vector2 start,
Func <Microsoft.Xna.Framework.Vector2, Microsoft.Xna.Framework.Vector2> supportA, Func <Microsoft.Xna.Framework.Vector2, Microsoft.Xna.Framework.Vector2> supportB)
{
simplex.Initialize();
Microsoft.Xna.Framework.Vector2 d = start;
Microsoft.Xna.Framework.Vector2 dperp = new Microsoft.Xna.Framework.Vector2(-d.Y, d.X);
double diff = double.MaxValue;
simplex.Add(MinkowskiSimplex.Support(supportA, supportB, dperp));
simplex.Add(MinkowskiSimplex.Support(supportA, supportB, d));
simplex.Add(MinkowskiSimplex.Support(supportA, supportB, -d));
d = LineSegmentPointNearestOrigin(simplex.Simplex[1], simplex.Simplex[2]);
for (int i = 1; i <= 2; i++)
{
var newd = LineSegmentPointNearestOrigin(simplex.Simplex[0], simplex.Simplex[i]);
if (newd.LengthSquared() < d.LengthSquared())
{
d = newd;
}
}
Iterations = 0;
Converged = false;
while (Iterations < MaxIterations && diff > Tolerance)
{
Iterations++;
if (simplex.ContainsOrigin)
{
Converged = true;
simplex.DistanceFromOrigin = d.Length();
return(simplex);
}
d = -d;
var c = MinkowskiSimplex.Support(supportA, supportB, d);
var dotc = Vector2.Dot(c.Difference, d);
var dota = Vector2.Dot(simplex.Last(), d);
diff = dotc - dota;
if (diff < Tolerance)
{
Converged = true;
simplex.DistanceFromOrigin = d.Length();
return(simplex);
}
var ia = simplex.Simplex.Count - 2;
var ib = simplex.Simplex.Count - 1;
var p1 = LineSegmentPointNearestOrigin(c.Difference, simplex.Simplex[ia]);
var p2 = LineSegmentPointNearestOrigin(c.Difference, simplex.Simplex[ib]);
if (p1.LengthSquared() < p2.LengthSquared())
{
simplex.StaggerInsert(ib, c);
d = p1;
}
else
{
simplex.StaggerInsert(ia, c);
d = p2;
}
}
simplex.DistanceFromOrigin = d.Length();
return(simplex);
}