LengthSquared() public method

Calculates the length of the vector squared.
public LengthSquared ( ) : float
return float
Beispiel #1
0
        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;
        }
Beispiel #2
0
        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
        }
Beispiel #4
0
        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;
        }
Beispiel #8
0
        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;
        }
Beispiel #9
0
        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;
        }
Beispiel #10
0
        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);
     }
 }
Beispiel #12
0
 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);
 }
Beispiel #13
0
        /// <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));
        }
Beispiel #14
0
        /// <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));
        }
Beispiel #15
0
 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;
      }
    }
Beispiel #20
0
        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;
        }
Beispiel #21
0
        /// <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;
            }
        }
Beispiel #23
0
        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;
        }
    }
Beispiel #25
0
        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;
            }
        }
Beispiel #26
0
 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;
 }
Beispiel #27
0
 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;
 }
Beispiel #28
0
        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);
        }
Beispiel #29
0
        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);
        }
Beispiel #30
0
        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);
 }
Beispiel #32
0
        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);
        }