System.Math.Abs(float)

public void SetPercent(float value) {
            if (value > 1) value = 1;
            else if (value < 0) value = 0;

            var difference = Math.Abs(value - LastPercent);
            if (difference == 0) return;


            Anim?.Delete();
            if (difference >= 0.01f) {
                Anim = Animation.AddAnim(f => SetPercentInternal(f), 0.2f, LastPercent, value);
            }
            else {
                SetPercentInternal(value);
            }
            LastPercent = value;
        }

public void Update(float deltaTime)
        {
            Current = Vector3.Lerp(Current, Goal, (Math.Abs(SmoothTime) < Mathf.Epsilon) ? 1.0f : deltaTime / SmoothTime);
        }

private bool GetCursorTargetAxes(Vector3 normal, ref Vector3 right, ref Vector3 up, ref Vector3 forward)
        {
            if (TargetedObject)
            {
                Vector3 objRight = TargetedObject.transform.TransformDirection(Vector3.right);
                Vector3 objUp = TargetedObject.transform.TransformDirection(Vector3.up);
                Vector3 objForward = TargetedObject.transform.TransformDirection(Vector3.forward);

                float dotRight = Vector3.Dot(normal, objRight);
                float dotUp = Vector3.Dot(normal, objUp);
                float dotForward = Vector3.Dot(normal, objForward);

                if (Math.Abs(dotRight) > Math.Abs(dotUp) &&
                    Math.Abs(dotRight) > Math.Abs(dotForward))
                {
                    forward = (dotRight > 0 ? objRight : -objRight).normalized;
                }
                else if (Math.Abs(dotUp) > Math.Abs(dotForward))
                {
                    forward = (dotUp > 0 ? objUp : -objUp).normalized;
                }
                else
                {
                    forward = (dotForward > 0 ? objForward : -objForward).normalized;
                }

                right = Vector3.Cross(Vector3.up, forward).normalized;
                if (right == Vector3.zero)
                {
                    right = Vector3.Cross(objForward, forward).normalized;
                }
                up = Vector3.Cross(forward, right).normalized;

                return true;
            }

            return false;
        }

void ReadTimeSyncMessage(ulong remoteID, byte[] msg)
		{
			if (!m_remoteSentTimeCache.ContainsKey(remoteID))
			{
				SendTimeSyncMessage(remoteID);
				return;
			}

			int offset = 1;
			float remoteTime = UnpackFloat(msg, ref offset);
			float now = Time.realtimeSinceStartup;
			float latency = (now - m_remoteSentTimeCache[remoteID]) / 2;
			float remoteTimeOffset = now - (remoteTime + latency);

			m_remoteSyncTimeCache[remoteID].Add(remoteTimeOffset);

			if (m_remoteSyncTimeCache[remoteID].Count < TIME_SYNC_MESSAGE_COUNT)
			{
				SendTimeSyncMessage(remoteID);
			}
			else
			{
				if (PlatformManager.MyID < remoteID)
				{
					// this client started the sync, need to send one last message to
					// the remote so they can finish their sync calculation
					SendTimeSyncMessage(remoteID);
				}

				// sort the times and remember the median
				m_remoteSyncTimeCache[remoteID].Sort();
				float median = m_remoteSyncTimeCache[remoteID][TIME_SYNC_MESSAGE_COUNT/2];

				// calucate the mean and standard deviation
				double mean = 0;
				foreach (var time in m_remoteSyncTimeCache[remoteID])
				{
					mean += time;
				}
				mean /= TIME_SYNC_MESSAGE_COUNT;

				double std_dev = 0;
				foreach (var time in m_remoteSyncTimeCache[remoteID])
				{
					std_dev += (mean-time)*(mean-time);
				}
				std_dev = Math.Sqrt(std_dev)/TIME_SYNC_MESSAGE_COUNT;

				// time delta is the mean of the values less than 1 standard deviation from the median
				mean = 0;
				int meanCount = 0;
				foreach (var time in m_remoteSyncTimeCache[remoteID])
				{
					if (Math.Abs(time-median) < std_dev)
					{
						mean += time;
						meanCount++;
					}
				}
				mean /= meanCount;
				Debug.LogFormat("Time offset to {0} is {1}", remoteID, mean);

				m_remoteSyncTimeCache.Remove(remoteID);
				m_remoteSentTimeCache.Remove(remoteID);
				m_remotePlayers[remoteID].remoteTimeOffset = (float)mean;

				// now that times are synchronized, lets try to coordinate the
				// start time for the match
				OfferMatchStartTime();
			}
		}

public static bool EpsilonEquals(this float num, float val/*, int decimalPlaces = 4*/)
        {
            //var epsilon = 1.0f / Math.Pow(10, decimalPlaces);
            var epsilon = 0.0001f;

            return Math.Abs(num - val) < epsilon;
        }

private void TreasureMaterialBase_Normalize(Dictionary<int, Dictionary<int, List<TreasureMaterialBase>>> materialBase)
        {
            foreach (var kvp in materialBase)
            {
                var materialCode = kvp.Key;
                var tiers = kvp.Value;

                foreach (var kvp2 in tiers)
                {
                    var tier = kvp2.Key;
                    var list = kvp2.Value;

                    var totalProbability = list.Sum(i => i.Probability);

                    if (Math.Abs(1.0f - totalProbability) < NormalizeEpsilon)
                        continue;

                    //Console.WriteLine($"TotalProbability {totalProbability} found for TreasureMaterialBase {materialCode} tier {tier}");

                    var factor = 1.0f / totalProbability;

                    foreach (var item in list)
                        item.Probability *= factor;

                    /*totalProbability = list.Sum(i => i.Probability);

                    Console.WriteLine($"After: {totalProbability}");*/
                }
            }
        }

private void TreasureMaterialColor_Normalize(Dictionary<int, Dictionary<int, List<TreasureMaterialColor>>> materialColor)
        {
            foreach (var kvp in materialColor)
            {
                var material = kvp.Key;
                var colorCodes = kvp.Value;

                foreach (var kvp2 in colorCodes)
                {
                    var colorCode = kvp2.Key;
                    var list = kvp2.Value;

                    var totalProbability = list.Sum(i => i.Probability);

                    if (Math.Abs(1.0f - totalProbability) < NormalizeEpsilon)
                        continue;

                    //Console.WriteLine($"TotalProbability {totalProbability} found for TreasureMaterialColor {(MaterialType)material} ColorCode {colorCode}");

                    var factor = 1.0f / totalProbability;

                    foreach (var item in list)
                        item.Probability *= factor;

                    /*totalProbability = list.Sum(i => i.Probability);

                    Console.WriteLine($"After: {totalProbability}");*/
                }
            }
        }

private void TreasureMaterialGroups_Normalize(Dictionary<int, Dictionary<int, List<TreasureMaterialGroups>>> materialGroups)
        {
            foreach (var kvp in materialGroups)
            {
                var materialGroup = kvp.Key;
                var tiers = kvp.Value;

                foreach (var kvp2 in tiers)
                {
                    var tier = kvp2.Key;
                    var list = kvp2.Value;

                    var totalProbability = list.Sum(i => i.Probability);

                    if (Math.Abs(1.0f - totalProbability) < NormalizeEpsilon)
                        continue;

                    //Console.WriteLine($"TotalProbability {totalProbability} found for TreasureMaterialGroup {(MaterialType)materialGroup} tier {tier}");

                    var factor = 1.0f / totalProbability;

                    foreach (var item in list)
                        item.Probability *= factor;

                    /*totalProbability = list.Sum(i => i.Probability);

                    Console.WriteLine($"After: {totalProbability}");*/
                }
            }
        }

public void apply_physics(AFrame frame, float quantum, float sign)
        {
            if (sign >= 0.0)
                quantum = Math.Abs(quantum);
            else
                quantum = -Math.Abs(quantum);

            frame.Origin += Velocity * quantum;
            frame.Rotate(Omega * quantum);
        }

public void update_internal(float timeElapsed, ref LinkedListNode<AnimSequenceNode> animNode, ref float frameNum, ref AFrame frame)
        {
            var currAnim = animNode.Value;

            var framerate = currAnim.Framerate;
            var frametime = framerate * timeElapsed;

            var lastFrame = (int)Math.Floor(frameNum);

            frameNum += frametime;
            var frameTimeElapsed = 0.0f;
            var animDone = false;

            if (frametime > 0.0f)
            {
                if (currAnim.get_high_frame() < Math.Floor(frameNum))
                {
                    var frameOffset = frameNum - currAnim.get_high_frame() - 1.0f;
                    if (frameOffset < 0.0f)
                        frameOffset = 0.0f;

                    if (Math.Abs(framerate) > PhysicsGlobals.EPSILON)
                        frameTimeElapsed = frameOffset / framerate;

                    frameNum = currAnim.get_high_frame();
                    animDone = true;
                }
                while (Math.Floor(frameNum) > lastFrame)
                {
                    if (frame != null)
                    {
                        if (currAnim.Anim.PosFrames != null)
                            frame = AFrame.Combine(frame, currAnim.get_pos_frame(lastFrame));

                        if (Math.Abs(framerate) > PhysicsGlobals.EPSILON)
                            apply_physics(frame, 1.0f / framerate, timeElapsed);
                    }

                    execute_hooks(currAnim.get_part_frame(lastFrame), AnimationHookDir.Forward);
                    lastFrame++;
                }
            }
            else if (frametime < 0.0f)
            {
                if (currAnim.get_low_frame() > Math.Floor(frameNum))
                {
                    var frameOffset = frameNum - currAnim.get_low_frame();
                    if (frameOffset > 0.0f)
                        frameOffset = 0.0f;

                    if (Math.Abs(framerate) > PhysicsGlobals.EPSILON)
                        frameTimeElapsed = frameOffset / framerate;

                    frameNum = currAnim.get_low_frame();
                    animDone = true;
                }
                while (Math.Floor(frameNum) < lastFrame)
                {
                    if (frame != null)
                    {
                        if (currAnim.Anim.PosFrames != null)
                            frame.Subtract(currAnim.get_pos_frame(lastFrame));

                        if (Math.Abs(framerate) > PhysicsGlobals.EPSILON)
                            apply_physics(frame, 1.0f / framerate, timeElapsed);
                    }

                    execute_hooks(currAnim.get_part_frame(lastFrame), AnimationHookDir.Backward);
                    lastFrame--;
                }
            }
            else
            {
                if (frame != null && Math.Abs(timeElapsed) > PhysicsGlobals.EPSILON)
                    apply_physics(frame, timeElapsed, timeElapsed);
            }

            if (!animDone)
                return;

            if (HookObj != null)
            {
                var node = AnimList.First;
                if (!node.Equals(FirstCyclic))
                    HookObj.add_anim_hook(AnimationHook.AnimDoneHook);
            }

            advance_to_next_animation(timeElapsed, ref animNode, ref frameNum, ref frame);
            timeElapsed = frameTimeElapsed;

            // loop to next anim
            update_internal(timeElapsed, ref animNode, ref frameNum, ref frame);    
        }

public void change_cycle_speed(Sequence sequence, MotionData motionData, float substateMod, float speedMod)
        {
            if (Math.Abs(substateMod) > PhysicsGlobals.EPSILON)
                sequence.multiply_cyclic_animation_framerate(speedMod / substateMod);

            else if (Math.Abs(speedMod) < PhysicsGlobals.EPSILON)
                sequence.multiply_cyclic_animation_framerate(0);
        }

public void apply_run_to_command(ref uint motion, ref float speed)
        {
            var speedMod = 1.0f;

            if (WeenieObj != null)
            {
                var runFactor = 0.0f;
                if (WeenieObj.InqRunRate(ref runFactor))
                    speedMod = runFactor;
                else
                    speedMod = MyRunRate;
            }
            switch (motion)
            {
                case (uint)MotionCommand.WalkForward:
                    if (speed > 0.0f)
                        motion = (uint)MotionCommand.RunForward;

                    speed *= speedMod;
                    break;

                case (uint)MotionCommand.TurnRight:
                    speed *= RunTurnFactor;
                    break;

                case (uint)MotionCommand.SideStepRight:
                    speed *= speedMod;

                    if (MaxSidestepAnimRate < Math.Abs(speed))
                    {
                        if (speed > 0.0f)
                            speed = MaxSidestepAnimRate * 1.0f;
                        else
                            speed = MaxSidestepAnimRate * -1.0f;
                    }
                    break;
            }
        }

public bool OnRoad(Vector3 obj)
        {
            int x = (int)(obj.X / TileLength);
            int y = (int)(obj.Y / TileLength);

            float rMin = RoadWidth;
            float rMax = TileLength - RoadWidth;

            int x0 = x;
            int x1 = x0 + 1;
            int y0 = y;
            int y1 = y0 + 1;

            uint r0 = GetRoad(x0, y0);
            uint r1 = GetRoad(x0, y1);
            uint r2 = GetRoad(x1, y0);
            uint r3 = GetRoad(x1, y1);

            if (r0 == 0 && r1 == 0 && r2 == 0 && r3 == 0)
                return false;

            float dx = obj.X - x * TileLength;
            float dy = obj.Y - y * TileLength;

            if (r0 > 0)
            {
                if (r1 > 0)
                {
                    if (r2 > 0)
                    {
                        if (r3 > 0)
                            return true;
                        else
                            return (dx < rMin || dy < rMin);
                    }
                    else
                    {
                        if (r3 > 0)
                            return (dx < rMin || dy > rMax);
                        else
                            return (dx < rMin);
                    }
                }
                else
                {
                    if (r2 > 0)
                    {
                        if (r3 > 0)
                            return (dx > rMax || dy < rMin);
                        else
                            return (dy < rMin);
                    }
                    else
                    {
                        if (r3 > 0)
                            return (Math.Abs(dx - dy) < rMin);
                        else
                            return (dx + dy < rMin);
                    }
                }
            }
            else
            {
                if (r1 > 0)
                {
                    if (r2 > 0)
                    {
                        if (r3 > 0)
                            return (dx > rMax || dy > rMax);
                        else
                            return (Math.Abs(dx + dy - TileLength) < rMin);
                    }
                    else
                    {
                        if (r3 > 0)
                            return (dy > rMax);
                        else
                            return (TileLength + dx - dy < rMin);
                    }
                }
                else
                {
                    if (r2 > 0)
                    {
                        if (r3 > 0)
                            return (dx > rMax);
                        else
                            return (TileLength - dx + dy < rMin);
                    }
                    else
                    {
                        if (r3 > 0)
                            return (TileLength * 2f - dx - dy < rMin);
                        else
                            return false;
                    }
                }
            }
        }

public static float heading_diff(float h1, float h2, uint motion)
        {
            var result = h1 - h2;

            if (Math.Abs(result) < PhysicsGlobals.EPSILON)
                result = 0.0f;
            if (result < -PhysicsGlobals.EPSILON)
                result += 360.0f;
            if (result > PhysicsGlobals.EPSILON && motion != (uint)MotionCommand.TurnRight)
                result = 360.0f - result;
            return result;
        }

public static bool heading_greater(float h1, float h2, uint motion)
        {
            /*var less = Math.Abs(x - y) <= 180.0f ? x < y : y < x;
            var result = (less || x == y) == false;
            if (motion != 0x6500000D)
                result = !result;
            return result;*/
            var diff = Math.Abs(h1 - h2);

            float v1, v2;

            if (diff <= 180.0f)
            {
                v1 = h2;
                v2 = h1;
            }
            else
            {
                v1 = h1;
                v2 = h2;
            }

            var result = (v2 > v1) ? true : false;

            if (motion != (uint)MotionCommand.TurnRight)
                result = !result;

            return result;
        }

public override void behaviorAtGameTick(GameTime time)
        {
            base.behaviorAtGameTick(time);
            this.faceDirection(0);
            float xSlope = (float)(-(float)(base.Player.GetBoundingBox().Center.X - this.GetBoundingBox().Center.X));
            float ySlope = (float)(base.Player.GetBoundingBox().Center.Y - this.GetBoundingBox().Center.Y);
            float t = Math.Max(1f, Math.Abs(xSlope) + Math.Abs(ySlope));
            if (t < 64f)
            {
                this.xVelocity = Math.Max(-7f, Math.Min(7f, this.xVelocity * 1.1f));
                this.yVelocity = Math.Max(-7f, Math.Min(7f, this.yVelocity * 1.1f));
            }
            xSlope /= t;
            ySlope /= t;
            if (this.wareplacedCounter <= 0)
            {
                this.targetRotation = (float)Math.Atan2((double)(-(double)ySlope), (double)xSlope) - 1.57079637f;
                if ((double)(Math.Abs(this.targetRotation) - Math.Abs(this.rotation)) > 2.748893571891069 && Game1.random.NextDouble() < 0.5)
                {
                    this.turningRight = true;
                }
                else if ((double)(Math.Abs(this.targetRotation) - Math.Abs(this.rotation)) < 0.39269908169872414)
                {
                    this.turningRight = false;
                }
                if (this.turningRight)
                {
                    this.rotation -= (float)Math.Sign(this.targetRotation - this.rotation) * 0.0490873866f;
                }
                else
                {
                    this.rotation += (float)Math.Sign(this.targetRotation - this.rotation) * 0.0490873866f;
                }
                this.rotation %= 6.28318548f;
                this.wareplacedCounter = 5 + Game1.random.Next(-1, 2);
            }
            float maxAccel = Math.Min(7f, Math.Max(2f, 7f - t / 64f / 2f))*2;
            xSlope = (float)Math.Cos((double)this.rotation + 1.5707963267948966);
            ySlope = -(float)Math.Sin((double)this.rotation + 1.5707963267948966);
            this.xVelocity += -xSlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
            this.yVelocity += -ySlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
            if (Math.Abs(this.xVelocity) > Math.Abs(-xSlope * 7f))
            {
                this.xVelocity -= -xSlope * maxAccel / 6f;
            }
            if (Math.Abs(this.yVelocity) > Math.Abs(-ySlope * 7f))
            {
                this.yVelocity -= -ySlope * maxAccel / 6f;
            }
        }

public override void behaviorAtGameTick(GameTime time)
        {
            invincibleCountdown = 1000;
            if (timeBeforeAIMovementAgain > 0f)
            {
                timeBeforeAIMovementAgain -= (float)time.ElapsedGameTime.Milliseconds;
            }
            if (lastHitCounter >= 0)
            {
                lastHitCounter.Value -= time.ElapsedGameTime.Milliseconds;
            }
            if (lastScreechCounter >= 0)
            {
                lastScreechCounter.Value -= time.ElapsedGameTime.Milliseconds;
            }
            if (lastScreechCounter < 0 && GetBoundingBox().Intersects(GetOwner().GetBoundingBox()))
            {
                if(ModEntry.Config.BatSoundEffects)
                    currentLocation.playSound("batScreech", NetAudio.SoundContext.Default);
                lastScreechCounter.Value = 10000;
            }

            chargingMonster = false;
            if(lastHitCounter < 0 && !(currentLocation is SlimeHutch))
            {
                foreach (NPC npc in currentLocation.characters)
                {
                    if (npc is Familiar)
                        continue;

                    if (npc is Monster && FamiliarsUtils.monstersColliding(this, (Monster)npc))
                    {
                        if (BaseDamage() >= 0)
                        {
                            int damageAmount = Game1.random.Next(BaseDamage(), BaseDamage() * 2 + 1);
                            damageAmount = (npc as Monster).takeDamage(damageAmount, 0, 0, false, 0, GetOwner());
                            if((npc as Monster).Health <= 0)
                            {
                                AddExp(10);
                            }
                            else
                            {
                                AddExp(1);
                            }
                        }
                        lastHitCounter.Value = AttackInterval();
                        chargingMonster = false;
                        break;
                    }
                    else if (npc is Monster && FamiliarsUtils.withinMonsterThreshold(this, (Monster)npc, 2))
                    {
                        chargingMonster = true;
                        if (currentTarget == null || Vector2.Distance(npc.position, position) < Vector2.Distance(currentTarget.position, position))
                        {
                            currentTarget = (Monster)npc;
                        }
                    }
                }
            }

            if (wareplacedCounter >= 0)
            {
                wareplacedCounter.Value -= time.ElapsedGameTime.Milliseconds;
            }

            if (chargingMonster || followingOwner)
            {
                seenPlayer.Value = true;

                Vector2 center = Position + new Vector2(8, 8);
                Vector2 playerCenter = GetOwner().position + new Vector2(64, 92);
                if (Vector2.Distance(playerCenter, center) > 256)
                {
                    Position = Vector2.Distance(playerCenter, center) * 0.03f * Vector2.Normalize(playerCenter - center) + center - new Vector2(8, 8);

                }

                float xSlope = (float)(-(float)(playerCenter.X - center.X));
                float ySlope = (float)(playerCenter.Y - center.Y);
                float t = Math.Max(1f, Math.Abs(xSlope) + Math.Abs(ySlope));
                if (t < (float)((extraVelocity > 0f) ? 192 : 64))
                {
                    xVelocity = Math.Max(-maxSpeed, Math.Min(maxSpeed, xVelocity * 1.05f));
                    yVelocity = Math.Max(-maxSpeed, Math.Min(maxSpeed, yVelocity * 1.05f));
                }
                xSlope /= t;
                ySlope /= t;
                if (wareplacedCounter <= 0)
                {
                    targetRotation = (float)Math.Atan2((double)(-(double)ySlope), (double)xSlope) - 1.57079637f;
                    if ((double)(Math.Abs(targetRotation) - Math.Abs(rotation)) > 2.748893571891069 && Game1.random.NextDouble() < 0.5)
                    {
                        turningRight.Value = true;
                    }
                    else if ((double)(Math.Abs(targetRotation) - Math.Abs(rotation)) < 0.39269908169872414)
                    {
                        turningRight.Value = false;
                    }
                    if (turningRight)
                    {
                        rotation -= (float)Math.Sign(targetRotation - rotation) * 0.0490873866f;
                    }
                    else
                    {
                        rotation += (float)Math.Sign(targetRotation - rotation) * 0.0490873866f;
                    }
                    rotation %= 6.28318548f;
                    wareplacedCounter.Value = 0;
                }
                float maxAccel = Math.Min(5f, Math.Max(1f, 5f - t / 64f / 2f)) + extraVelocity;
                xSlope = (float)Math.Cos((double)rotation + 1.5707963267948966);
                ySlope = -(float)Math.Sin((double)rotation + 1.5707963267948966);
                xVelocity += -xSlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
                yVelocity += -ySlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
                if (Math.Abs(xVelocity) > Math.Abs(-xSlope * maxSpeed))
                {
                    xVelocity -= -xSlope * maxAccel / 6f;
                }
                if (Math.Abs(yVelocity) > Math.Abs(-ySlope * maxSpeed))
                {
                    yVelocity -= -ySlope * maxAccel / 6f;
                }
            }
        }

public override void behaviorAtGameTick(GameTime time)
        {
            invincibleCountdown = 1000;
            if (timeBeforeAIMovementAgain > 0f)
            {
                timeBeforeAIMovementAgain -= time.ElapsedGameTime.Milliseconds;
            }
            if (lastBuff >= 0)
            {
                lastBuff.Value -= time.ElapsedGameTime.Milliseconds;
            }

            if (wareplacedCounter >= 0)
            {
                wareplacedCounter.Value -= time.ElapsedGameTime.Milliseconds;
            }

            if (followingOwner)
            {

                Vector2 center = Position + new Vector2(8, 8);
                Vector2 playerCenter = GetOwner().position + new Vector2(64, 92);
                if (Vector2.Distance(playerCenter, center) > 256)
                {
                    Position = Vector2.Distance(playerCenter, center) * 0.03f * Vector2.Normalize(playerCenter - center) + center - new Vector2(8,8);

                }

                float xSlope = (float)(-(float)(playerCenter.X - center.X));
                float ySlope = (float)(playerCenter.Y - center.Y);
                float t = Math.Max(1f, Math.Abs(xSlope) + Math.Abs(ySlope));
                if (t < (float)((extraVelocity > 0f) ? 192 : 64))
                {
                    xVelocity = Math.Max(-maxSpeed, Math.Min(maxSpeed, xVelocity * 1.05f));
                    yVelocity = Math.Max(-maxSpeed, Math.Min(maxSpeed, yVelocity * 1.05f));
                }
                xSlope /= t;
                ySlope /= t;
                if (wareplacedCounter <= 0)
                {
                    targetRotation = (float)Math.Atan2((double)(-(double)ySlope), (double)xSlope) - 1.57079637f;
                    if ((double)(Math.Abs(targetRotation) - Math.Abs(rotation)) > 2.748893571891069 && Game1.random.NextDouble() < 0.5)
                    {
                        turningRight.Value = true;
                    }
                    else if ((double)(Math.Abs(targetRotation) - Math.Abs(rotation)) < 0.39269908169872414)
                    {
                        turningRight.Value = false;
                    }
                    if (turningRight)
                    {
                        rotation -= (float)Math.Sign(targetRotation - rotation) * 0.0490873866f;
                    }
                    else
                    {
                        rotation += (float)Math.Sign(targetRotation - rotation) * 0.0490873866f;
                    }
                    rotation %= 6.28318548f;
                    wareplacedCounter.Value = 0;
                }
                float maxAccel = Math.Min(5f, Math.Max(1f, 5f - t / 64f / 2f)) + extraVelocity;
                xSlope = (float)Math.Cos((double)rotation + 1.5707963267948966);
                ySlope = -(float)Math.Sin((double)rotation + 1.5707963267948966);
                xVelocity += -xSlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
                yVelocity += -ySlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
                if (Math.Abs(xVelocity) > Math.Abs(-xSlope * maxSpeed))
                {
                    xVelocity -= -xSlope * maxAccel / 6f;
                }
                if (Math.Abs(yVelocity) > Math.Abs(-ySlope * maxSpeed))
                {
                    yVelocity -= -ySlope * maxAccel / 6f;
                }
                if (lastBuff <= 0 && Vector2.Distance(GetOwner().getTileLocation(), getTileLocation()) < 3)
                {
                    if (Game1.random.NextDouble() < BuffChance())
                    {
                        if (ModEntry.Config.ButterflySoundEffects)
                            Game1.playSound("yoba");
                        BuffsDisplay buffsDisplay = Game1.buffsDisplay;
                        Buff buff2 = GetBuff();
                        buffsDisplay.addOtherBuff(buff2);
                        AddExp(1);
                        lastBuff.Value = GetBuffInterval();
                    }
                    else
                        lastBuff.Value = 1000;
                }
            }
        }

protected override void updateAnimation(GameTime time)
        {
            base.updateAnimation(time);
            if (wareplacedCounter >= 0)
            {
                wareplacedCounter -= time.ElapsedGameTime.Milliseconds;
            }
            Sprite.Animate(time, 0, 9, 40f);
            float xSlope = (float)(-(float)(base.Player.GetBoundingBox().Center.X - GetBoundingBox().Center.X));
            float ySlope = (float)(base.Player.GetBoundingBox().Center.Y - GetBoundingBox().Center.Y);
            float t = Math.Max(1f, Math.Abs(xSlope) + Math.Abs(ySlope));
            if (t < 64f)
            {
                xVelocity = Math.Max(-7f, Math.Min(7f, xVelocity * 1.1f));
                yVelocity = Math.Max(-7f, Math.Min(7f, yVelocity * 1.1f));
            }
            xSlope /= t;
            ySlope /= t;
            if (wareplacedCounter <= 0)
            {
                targetRotation = (float)Math.Atan2((double)(-(double)ySlope), (double)xSlope) - 1.57079637f;
                if ((double)(Math.Abs(targetRotation) - Math.Abs(rotation)) > 2.748893571891069 && Game1.random.NextDouble() < 0.5)
                {
                    turningRight = true;
                }
                else if ((double)(Math.Abs(targetRotation) - Math.Abs(rotation)) < 0.39269908169872414)
                {
                    turningRight = false;
                }
                if (turningRight)
                {
                    rotation -= (float)Math.Sign(targetRotation - rotation) * 0.0490873866f;
                }
                else
                {
                    rotation += (float)Math.Sign(targetRotation - rotation) * 0.0490873866f;
                }
                rotation %= 6.28318548f;
                wareplacedCounter = 5 + Game1.random.Next(-1, 2);
            }
            float maxAccel = Math.Min(7f, Math.Max(2f, 7f - t / 64f / 2f));
            xSlope = (float)Math.Cos((double)rotation + 1.5707963267948966);
            ySlope = -(float)Math.Sin((double)rotation + 1.5707963267948966);
            xVelocity += -xSlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
            yVelocity += -ySlope * maxAccel / 6f + (float)Game1.random.Next(-10, 10) / 100f;
            if (Math.Abs(xVelocity) > Math.Abs(-xSlope * 7f))
            {
                xVelocity -= -xSlope * maxAccel / 6f;
            }
            if (Math.Abs(yVelocity) > Math.Abs(-ySlope * 7f))
            {
                yVelocity -= -ySlope * maxAccel / 6f;
            }
            base.resetAnimationSpeed();
        }

private static async void HereFishyFishy(Farmer who, int x, int y)
        {
            hereFishying = true;
            if (fishySound != null)
            {
                fishySound.Play();
            }
            who.completelyStopAnimatingOrDoingAction();
            who.jitterStrength = 2f;
            List<FarmerSprite.AnimationFrame> animationFrames = new List<FarmerSprite.AnimationFrame>(){
                new FarmerSprite.AnimationFrame(94, 100, false, false, null, false).AddFrameAction(delegate (Farmer f)
                {
                    f.jitterStrength = 2f;
                })
            };
            who.FarmerSprite.setCurrentAnimation(animationFrames.ToArray());
            who.FarmerSprite.PauseForSingleAnimation = true;
            who.FarmerSprite.loop = true;
            who.FarmerSprite.loopThisAnimation = true;
            who.Sprite.currentFrame = 94;

            await System.Threading.Tasks.Task.Delay(1793);

            canPerfect = true;
            perfect = false;

            who.synchronizedJump(8f);

            await System.Threading.Tasks.Task.Delay(100);

            canPerfect = false;

            await System.Threading.Tasks.Task.Delay(900);

            who.stopJittering();
            who.completelyStopAnimatingOrDoingAction();
            who.forceCanMove();

            hereFishying = false;

            await System.Threading.Tasks.Task.Delay(Game1.random.Next(500, 1000));

            Object o = who.currentLocation.getFish(0, -1, 1, who, 0, new Vector2(x, y), who.currentLocation.Name);
            if (o == null || o.ParentSheetIndex <= 0)
            {
                o = new Object(Game1.random.Next(167, 173), 1, false, -1, 0);
            }


            int parentSheetIndex = o.parentSheetIndex;
            animations.Clear();
            float t;
            lastUser = who;
            whichFish = parentSheetIndex;
            Dictionary<int, string> data = Game1.content.Load<Dictionary<int, string>>("Data\\Fish");
            string[] datas = null;
            if (data.ContainsKey(whichFish))
            {
                datas = data[whichFish].Split('/');
            }


            bool non_fishable_fish = false;
            if (o is Furniture)
            {
                non_fishable_fish = true;
            }
            else if (Utility.IsNormalObjectAtParentSheetIndex(o, o.ParentSheetIndex) && data.ContainsKey(o.ParentSheetIndex))
            {
                string[] array = data[o.ParentSheetIndex].Split(new char[]
                {
                            '/'
                });
                int difficulty = -1;
                if (!int.TryParse(array[1], out difficulty))
                {
                    non_fishable_fish = true;
                }
            }
            else
            {
                non_fishable_fish = true;
            }


            float fs = 1f;
            int minimumSizeContribution = 1 + who.FishingLevel / 2;
            fs *= (float)Game1.random.Next(minimumSizeContribution, Math.Max(6, minimumSizeContribution)) / 5f;
            fs *= 1.2f;
            fs *= 1f + (float)Game1.random.Next(-10, 11) / 100f;
            fs = Math.Max(0f, Math.Min(1f, fs));
            if(datas != null && !non_fishable_fish)
            {
                try
                {
                    int minFishSize = int.Parse(datas[3]);
                    int maxFishSize = int.Parse(datas[4]);
                    fishSize = (int)((float)minFishSize + (float)(maxFishSize - minFishSize) * fishSize);
                    fishSize++;
                    fishQuality = (((double)fishSize < 0.33) ? 0 : (((double)fishSize < 0.66) ? 1 : 2));
                    if (perfect)
                        fishQuality *= 2;
                }
                catch 
                {
                    context.Monitor.Log($"Error getting fish size from {data[whichFish]}", LogLevel.Error);
                }
            }
            bossFish = FishingRod.isFishBossFish(whichFish);
            caughtDoubleFish = !bossFish && Game1.random.NextDouble() < 0.1 + Game1.player.DailyLuck / 2.0;

            context.Monitor.Log($"pulling fish {whichFish} {fishSize} {who.Name} {x},{y}");

            if (who.IsLocalPlayer)
            {
                if (datas != null && !non_fishable_fish)
                {
                    fishDifficulty = int.Parse(datas[1]);

                }
                else
                    fishDifficulty = 0;
                
                int experience = Math.Max(1, (fishQuality + 1) * 3 + fishDifficulty / 3);
                if (bossFish)
                {
                    experience *= 5;
                }
                
                if(perfect)
                    experience += (int)((float)experience * 1.4f);
                
                who.gainExperience(1, experience);
            }

            if (who.FacingDirection == 1 || who.FacingDirection == 3)
            {
                float distance = Vector2.Distance(new Vector2(x, y), who.Position);
                float gravity = 0.001f;
                float height = 128f - (who.Position.Y - y + 10f);
                double angle = 1.1423973285781066;
                float yVelocity = (float)((double)(distance * gravity) * Math.Tan(angle) / Math.Sqrt((double)(2f * distance * gravity) * Math.Tan(angle) - (double)(2f * gravity * height)));
                if (float.IsNaN(yVelocity))
                {
                    yVelocity = 0.6f;
                }
                float xVelocity = (float)((double)yVelocity * (1.0 / Math.Tan(angle)));
                t = distance / xVelocity;
                animations.Add(new TemporaryAnimatedSprite("Maps\\springobjects", Game1.getSourceRectForStandardTileSheet(Game1.objectSpriteSheet, parentSheetIndex, 16, 16), t, 1, 0, new Vector2(x,y), false, false, y / 10000f, 0f, Color.White, 4f, 0f, 0f, 0f, false)
                {
                    motion = new Vector2((float)((who.FacingDirection == 3) ? -1 : 1) * -xVelocity, -yVelocity),
                    acceleration = new Vector2(0f, gravity),
                    timeBasedMotion = true,
                    endFunction = new TemporaryAnimatedSprite.endBehavior(playerCaughtFishEndFunction),
                    extraInfoForEndBehavior = parentSheetIndex,
                    endSound = "tinyWhip"
                });
                if (caughtDoubleFish)
                {
                    distance = Vector2.Distance(new Vector2(x, y), who.Position);
                    gravity = 0.0008f;
                    height = 128f - (who.Position.Y - y + 10f);
                    angle = 1.1423973285781066;
                    yVelocity = (float)((double)(distance * gravity) * Math.Tan(angle) / Math.Sqrt((double)(2f * distance * gravity) * Math.Tan(angle) - (double)(2f * gravity * height)));
                    if (float.IsNaN(yVelocity))
                    {
                        yVelocity = 0.6f;
                    }
                    xVelocity = (float)((double)yVelocity * (1.0 / Math.Tan(angle)));
                    t = distance / xVelocity;
                    animations.Add(new TemporaryAnimatedSprite("Maps\\springobjects", Game1.getSourceRectForStandardTileSheet(Game1.objectSpriteSheet, parentSheetIndex, 16, 16), t, 1, 0, new Vector2(x, y), false, false, y / 10000f, 0f, Color.White, 4f, 0f, 0f, 0f, false)
                    {
                        motion = new Vector2((float)((who.FacingDirection == 3) ? -1 : 1) * -xVelocity, -yVelocity),
                        acceleration = new Vector2(0f, gravity),
                        timeBasedMotion = true,
                        endSound = "fishSlap",
                        Parent = who.currentLocation
                    });
                }
            }
            else
            {
                float distance2 = y - (float)(who.getStandingY() - 64);
                float height2 = Math.Abs(distance2 + 256f + 32f);
                if (who.FacingDirection == 0)
                {
                    height2 += 96f;
                }
                float gravity2 = 0.003f;
                float velocity = (float)Math.Sqrt((double)(2f * gravity2 * height2));
                t = (float)(Math.Sqrt((double)(2f * (height2 - distance2) / gravity2)) + (double)(velocity / gravity2));
                float xVelocity2 = 0f;
                if (t != 0f)
                {
                    xVelocity2 = (who.Position.X - x) / t;
                }
                animations.Add(new TemporaryAnimatedSprite("Maps\\springobjects", Game1.getSourceRectForStandardTileSheet(Game1.objectSpriteSheet, parentSheetIndex, 16, 16), t, 1, 0, new Vector2(x, y), false, false, y / 10000f, 0f, Color.White, 4f, 0f, 0f, 0f, false)
                {
                    motion = new Vector2(xVelocity2, -velocity),
                    acceleration = new Vector2(0f, gravity2),
                    timeBasedMotion = true,
                    endFunction = new TemporaryAnimatedSprite.endBehavior(playerCaughtFishEndFunction),
                    extraInfoForEndBehavior = parentSheetIndex,
                    endSound = "tinyWhip"
                });
                if (caughtDoubleFish)
                {
                    distance2 = y - (float)(who.getStandingY() - 64);
                    height2 = Math.Abs(distance2 + 256f + 32f);
                    if (who.FacingDirection == 0)
                    {
                        height2 += 96f;
                    }
                    gravity2 = 0.004f;
                    velocity = (float)Math.Sqrt((double)(2f * gravity2 * height2));
                    t = (float)(Math.Sqrt((double)(2f * (height2 - distance2) / gravity2)) + (double)(velocity / gravity2));
                    xVelocity2 = 0f;
                    if (t != 0f)
                    {
                        xVelocity2 = (who.Position.X - x) / t;
                    }
                    animations.Add(new TemporaryAnimatedSprite("Maps\\springobjects", Game1.getSourceRectForStandardTileSheet(Game1.objectSpriteSheet, parentSheetIndex, 16, 16), t, 1, 0, new Vector2(x, y), false, false, y / 10000f, 0f, Color.White, 4f, 0f, 0f, 0f, false)
                    {
                        motion = new Vector2(xVelocity2, -velocity),
                        acceleration = new Vector2(0f, gravity2),
                        timeBasedMotion = true,
                        endSound = "fishSlap",
                        Parent = who.currentLocation
                    });
                }
            }
        }

public static void AbigailCaveTick()
        {

            Game1.player.CurrentToolIndex = Game1.player.items.Count;

            List<NPC> list = Game1.player.currentLocation.characters.ToList().FindAll((n) => (n is Monster) && (n as Monster).Health <= 0);
            foreach(NPC n in list)
            {
                Game1.player.currentLocation.characters.Remove(n);
            }

            if (abigailTicks.Value < 0)
            {
                return;
            }
            Game1.exitActiveMenu();

            if (abigailTicks.Value == 0)
            {
                FieldInfo f1 = Game1.player.currentLocation.characters.GetType().GetField("OnValueRemoved", BindingFlags.Instance | BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Static);
                f1.SetValue(Game1.player.currentLocation.characters, null);
            }

            Vector2 v = Vector2.Zero;
            float yrt = (float)(1/Math.Sqrt(2));
            if (Helper.Input.IsDown(SButton.Up) || Helper.Input.IsDown(SButton.RightThumbstickUp))
            {
                if(Helper.Input.IsDown(SButton.Right) || Helper.Input.IsDown(SButton.RightThumbstickRight))
                    v = new Vector2(yrt, -yrt);
                else if(Helper.Input.IsDown(SButton.Left) || Helper.Input.IsDown(SButton.RightThumbstickLeft))
                    v = new Vector2(-yrt, -yrt);
                else 
                    v = new Vector2(0, -1);
            }
            else if (Helper.Input.IsDown(SButton.Down) || Helper.Input.IsDown(SButton.RightThumbstickDown))
            {
                if(Helper.Input.IsDown(SButton.Right) || Helper.Input.IsDown(SButton.RightThumbstickRight))
                    v = new Vector2(yrt, yrt);
                else if(Helper.Input.IsDown(SButton.Left) || Helper.Input.IsDown(SButton.RightThumbstickLeft))
                    v = new Vector2(-yrt, yrt);
                else 
                    v = new Vector2(0, 1);
            }
            else if (Helper.Input.IsDown(SButton.Right) || Helper.Input.IsDown(SButton.RightThumbstickDown))
                v = new Vector2(1, 0);
            else if (Helper.Input.IsDown(SButton.Left) || Helper.Input.IsDown(SButton.RightThumbstickLeft))
                v = new Vector2(-1, 0);
            else if (Helper.Input.IsDown(SButton.MouseLeft))
            {
                float x = Game1.viewport.X + Game1.getOldMouseX() - Game1.player.position.X;
                float y = Game1.viewport.Y + Game1.getOldMouseY() - Game1.player.position.Y;
                float dx = Math.Abs(x);
                float dy = Math.Abs(y);
                if(y < 0)
                {
                    if(x > 0)
                    {
                        if(dy > dx)
                        {
                            if (dy-dx > dy / 2)
                                v = new Vector2(0, -1);
                            else
                                v = new Vector2(yrt, -yrt);

                        }
                        else
                        {
                            if (dx - dy > x / 2)
                                v = new Vector2(1, 0);
                            else
                                v = new Vector2(yrt, -yrt);
                        }
                    }
                    else
                    {
                        if (dy > dx)
                        {
                            if (dy - dx > dy / 2)
                                v = new Vector2(0, -1);
                            else
                                v = new Vector2(-yrt, -yrt);

                        }
                        else
                        {
                            if (dx - dy > x / 2)
                                v = new Vector2(-1, 0);
                            else
                                v = new Vector2(-yrt, -yrt);
                        }
                    }
                }
                else
                {
                    if (x > 0)
                    {
                        if (dy > dx)
                        {
                            if (dy - dx > dy / 2)
                                v = new Vector2(0, 1);
                            else
                                v = new Vector2(yrt, yrt);

                        }
                        else
                        {
                            if (dx - dy > x / 2)
                                v = new Vector2(1, 0);
                            else
                                v = new Vector2(yrt, yrt);
                        }
                    }
                    else
                    {
                        if (dy > dx)
                        {
                            if (dy - dx > dy / 2)
                                v = new Vector2(0, -1);
                            else
                                v = new Vector2(-yrt, yrt);

                        }
                        else
                        {
                            if (dx - dy > x / 2)
                                v = new Vector2(-1, 0);
                            else
                                v = new Vector2(-yrt, yrt);
                        }
                    }
                }
            }

            if (v != Vector2.Zero && Game1.player.millisecondsPlayed - lastProjectile.Value > 350)
            {
                Game1.player.currentLocation.projectiles.Add(new AbigailProjectile(1, 383, 0, 0, 0, v.X * 6, v.Y * 6, new Vector2(Game1.player.getStandingX() - 24, Game1.player.getStandingY() - 48), "Cowboy_monsterDie", "Cowboy_gunshot", false, true, Game1.player.currentLocation, Game1.player, true)); 
                lastProjectile.Value = Game1.player.millisecondsPlayed;
            }

            foreach (SButton button in abigailShootButtons)
            {
                if (Helper.Input.IsDown(button))
                {
                    switch (button)
                    {
                        case SButton.Up:
                            break;
                        case SButton.Right:
                            v = new Vector2(1, 0);
                            break;
                        case SButton.Down:
                            v = new Vector2(0, 1);
                            break;
                        default:
                            v = new Vector2(-1, 0);
                            break;
                    }
                }
            }


            abigailTicks.Value++;
            if(abigailTicks.Value > 80000 / 16f)
            {
                if (Game1.player.currentLocation.characters.ToList().FindAll((n) => (n is Monster)).Count > 0)
                    return;

                abigailTicks.Value = -1;
                Game1.player.hat.Value = null;
                Game1.stopMusicTrack(Game1.MusicContext.Default);

                if(!Game1.player.mailReceived.Contains("ScubaFins"))
                {
                    Game1.playSound("Cowboy_Secret");
                    SwimMaps.AddScubaChest(Game1.player.currentLocation, new Vector2(8, 8), "ScubaFins");
                }

                Game1.player.currentLocation.setMapTile(8, 16, 91, "Buildings", null);
                Game1.player.currentLocation.setMapTile(9, 16, 92, "Buildings", null);
                Game1.player.currentLocation.setTileProperty(9, 16, "Back", "Water", "T");
                Game1.player.currentLocation.setMapTile(10, 16, 93, "Buildings", null);
                Game1.player.currentLocation.setMapTile(8, 17, 107, "Buildings", null);
                Game1.player.currentLocation.setMapTile(9, 17, 108, "Back", null);
                Game1.player.currentLocation.setTileProperty(9, 17, "Back", "Water", "T");
                Game1.player.currentLocation.removeTile(9, 17, "Buildings");
                Game1.player.currentLocation.setMapTile(10, 17, 109, "Buildings", null); 
                Game1.player.currentLocation.setMapTile(8, 18, 139, "Buildings", null);
                Game1.player.currentLocation.setMapTile(9, 18, 140, "Buildings", null);
                Game1.player.currentLocation.setMapTile(10, 18, 141, "Buildings", null);
                SwimMaps.AddWaterTiles(Game1.player.currentLocation);
            }
            else
            {
                if (Game1.random.NextDouble() < 0.03)
                {
                    int which = Game1.random.Next(3);
                    Point p = new Point();
                    switch (Game1.random.Next(4))
                    {
                        case 0:
                            p = new Point(8 + which, 1);
                            break;
                        case 1:
                            p = new Point(1, 8 + which);
                            break;
                        case 2:
                            p = new Point(8 + which, 16);
                            break;
                        case 3:
                            p = new Point(16, 8 + which);
                            break;
                    }
                    Game1.player.currentLocation.characters.Add(new AbigailMetalHead(new Vector2(p.X * Game1.tileSize, p.Y * Game1.tileSize), 0));
                }

            }
        }

public static Vector2 getAwayFromNPCTrajectory(Microsoft.Xna.Framework.Rectangle monsterBox, NPC who)
        {
            float num = (float)(-(float)(who.GetBoundingBox().Center.X - monsterBox.Center.X));
            float ySlope = (float)(who.GetBoundingBox().Center.Y - monsterBox.Center.Y);
            float total = Math.Abs(num) + Math.Abs(ySlope);
            if (total < 1f)
            {
                total = 5f;
            }
            float x = num / total * (float)(50 + Game1.random.Next(-20, 20));
            ySlope = ySlope / total * (float)(50 + Game1.random.Next(-20, 20));
            return new Vector2(x, ySlope);
        }

int IComparer.Compare(object x, object y)
    {
        float num = (float)((PVPcheckPoint)x).id;
        float num2 = (float)((PVPcheckPoint)y).id;
        if (num == num2 || Math.Abs(num - num2) < 1.401298E-45f)
        {
            return 0;
        }
        if (num < num2)
        {
            return -1;
        }
        return 1;
    }

int IComparer.Compare(object x, object y)
    {
        float time = ((RacingResult)x).Time;
        float time2 = ((RacingResult)y).Time;
        if (time == time2 || Math.Abs(time - time2) < 1.401298E-45f)
        {
            return 0;
        }
        if (time < time2)
        {
            return -1;
        }
        return 1;
    }

public override VoronoiRegion GetRegion(ref ContactData contact)
        {
            //Deep contact can produce non-triangle normals while still being within the triangle.
            //To solve this problem, find the voronoi region to which the contact belongs using its normal.
            //The voronoi region will be either the most extreme vertex, or the edge that includes
            //the first and second most extreme vertices.
            //If the normal dotted with an extreme edge direction is near 0, then it belongs to the edge.
            //Otherwise, it belongs to the vertex.
            //MPR tends to produce 'approximate' normals, though.
            //Use a fairly forgiving epsilon.
            float dotA, dotB, dotC;
            Vector3.Dot(ref triangle.vA, ref contact.Normal, out dotA);
            Vector3.Dot(ref triangle.vB, ref contact.Normal, out dotB);
            Vector3.Dot(ref triangle.vC, ref contact.Normal, out dotC);

            //Since normal points from convex to triangle always, reverse dot signs.
            dotA = -dotA;
            dotB = -dotB;
            dotC = -dotC;


            float faceEpsilon = .01f;
            const float edgeEpsilon = .01f;

            float edgeDot;
            Vector3 edgeDirection;
            if (dotA > dotB && dotA > dotC)
            {
                //A is extreme.
                if (dotB > dotC)
                {
                    //B is second most extreme.
                    if (Math.Abs(dotA - dotC) < faceEpsilon)
                    {
                        //The normal is basically a face normal.  This can happen at the edges occasionally.
                        return VoronoiRegion.ABC;
                    }
                    else
                    {
                        Vector3.Subtract(ref triangle.vB, ref triangle.vA, out edgeDirection);
                        Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
                        if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
                            return VoronoiRegion.AB;
                        else
                            return VoronoiRegion.A;
                    }
                }
                else
                {
                    //C is second most extreme.
                    if (Math.Abs(dotA - dotB) < faceEpsilon)
                    {
                        //The normal is basically a face normal.  This can happen at the edges occasionally.
                        return VoronoiRegion.ABC;
                    }
                    else
                    {
                        Vector3.Subtract(ref triangle.vC, ref triangle.vA, out edgeDirection);
                        Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
                        if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
                            return VoronoiRegion.AC;
                        else
                            return VoronoiRegion.A;
                    }
                }
            }
            else if (dotB > dotC)
            {
                //B is extreme.
                if (dotC > dotA)
                {
                    //C is second most extreme.
                    if (Math.Abs(dotB - dotA) < faceEpsilon)
                    {
                        //The normal is basically a face normal.  This can happen at the edges occasionally.
                        return VoronoiRegion.ABC;
                    }
                    else
                    {
                        Vector3.Subtract(ref triangle.vC, ref triangle.vB, out edgeDirection);
                        Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
                        if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
                            return VoronoiRegion.BC;
                        else
                            return VoronoiRegion.B;
                    }
                }
                else
                {
                    //A is second most extreme.
                    if (Math.Abs(dotB - dotC) < faceEpsilon)
                    {
                        //The normal is basically a face normal.  This can happen at the edges occasionally.
                        return VoronoiRegion.ABC;
                    }
                    else
                    {
                        Vector3.Subtract(ref triangle.vA, ref triangle.vB, out edgeDirection);
                        Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
                        if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
                            return VoronoiRegion.AB;
                        else
                            return VoronoiRegion.B;
                    }
                }
            }
            else
            {
                //C is extreme.
                if (dotA > dotB)
                {
                    //A is second most extreme.
                    if (Math.Abs(dotC - dotB) < faceEpsilon)
                    {
                        //The normal is basically a face normal.  This can happen at the edges occasionally.
                        return VoronoiRegion.ABC;
                    }
                    else
                    {
                        Vector3.Subtract(ref triangle.vA, ref triangle.vC, out edgeDirection);
                        Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
                        if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
                            return VoronoiRegion.AC;
                        else
                            return VoronoiRegion.C;
                    }
                }
                else
                {
                    //B is second most extreme.
                    if (Math.Abs(dotC - dotA) < faceEpsilon)
                    {
                        //The normal is basically a face normal.  This can happen at the edges occasionally.
                        return VoronoiRegion.ABC;
                    }
                    else
                    {
                        Vector3.Subtract(ref triangle.vB, ref triangle.vC, out edgeDirection);
                        Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
                        if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
                            return VoronoiRegion.BC;
                        else
                            return VoronoiRegion.C;
                    }
                }
            }

        }

public override void Update(float dt)
        {
            //First, refresh all existing contacts.  This is an incremental manifold.
            var transform = MeshTransform;
            ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove);

            RemoveQueuedContacts();


            CleanUpOverlappingTriangles();
            //Get all the overlapped triangle indices.
            int triangleCount = FindOverlappingTriangles(dt);

            Matrix3x3 orientation;
            Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
            var guaranteedContacts = 0;
            for (int i = 0; i < triangleCount; i++)
            {
                //Initialize the local triangle.
                TriangleIndices indices;
                if (ConfigureTriangle(i, out indices))
                {

                    //Find a pairtester for the triangle.
                    TrianglePairTester pairTester;
                    if (!activePairTesters.TryGetValue(indices, out pairTester))
                    {
                        pairTester = GetTester();
                        pairTester.Initialize(convex.Shape, localTriangleShape);
                        activePairTesters.Add(indices, pairTester);
                    }
                    pairTester.Updated = true;


                    //Put the triangle into the local space of the convex.
                    Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
                    Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
                    Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
                    Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
                    Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
                    Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);

                    //Now, generate a contact between the two shapes.
                    ContactData contact;
                    TinyStructList<ContactData> contactList;
                    if (pairTester.GenerateContactCandidate(out contactList))
                    {
                        for (int j = 0; j < contactList.Count; j++)
                        {
                            contactList.Get(j, out contact);


                            if (UseImprovedBoundaryHandling)
                            {
                                if (replacedyzeCandidate(ref indices, pairTester, ref contact))
                                {
                                    //This is let through if there's a face contact. Face contacts cannot be blocked.
                                    guaranteedContacts++;
                                    AddLocalContact(ref contact, ref orientation);
                                }
                            }
                            else
                            {
                                AddLocalContact(ref contact, ref orientation);
                            }

                        }
                    }

                    //Get the voronoi region from the contact candidate generation.  Possibly just recalculate, since most of the systems don't calculate it.
                    //Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region.  For face contacts, don't bother- just add it if unique.
                    //For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices.
                    //For A, B, or C, add the index of the vertex to the blockedVertexRegions set.
                    //If the edge/vertex is already present in the set, then DO NOT add the contact.
                    //When adding a contact, add ALL other voronoi regions to the blocked sets. 
                }

            }



            if (UseImprovedBoundaryHandling)
            {

                //If there were no face contacts that absolutely must be included, we may get into a very rare situation
                //where absolutely no contacts get created.  For example, a sphere falling directly on top of a vertex in a flat terrain.
                //It will generally get locked out of usage by belonging only to restricted regions (numerical issues make it visible by both edges and vertices).
                //In some cases, the contacts will be ignored instead of corrected (e.g. spheres).
                //To prevent objects from just falling through the ground in such a situation, force-correct the contacts regardless of the pair tester's desires.
                //Sure, it might not be necessary under normal cirreplacedstances, but it's a better option than having no contacts.
                //TODO: There is another option: Changing restricted regions so that a vertex only restricts the other two vertices and the far edge,
                //and an edge only restricts the far vertex and other two edges.  This introduces an occasional bump though...

                //It's possible, in very specific instances, for an object to wedge itself between two adjacent triangles.
                //For this state to continue beyond a brief instant generally requires the object be orientation locked and slender.
                //However, some characters fit this description, so it can't be ignored!

                //Conceptually, this issue can occur at either a vertex junction or a shared edge (usually on extremely flat surfaces only).
                //However, an object stuck between multiple triangles is not in a stable state.  In the edge case, the object gets shoved to one side
                //as one contact 'wins' the solver war.  That's not enough to escape, unfortunately.
                //The vertex case, on the other hand, is degenerate and decays into an edge case rapidly thanks to this lack of stability.
                //So, we don't have to explicitly handle the somewhat more annoying and computationally expensive vertex unstucking case, because the edge case handles both! :)

                //This isn't a completely free operation, but it's guarded behind pretty rare conditions.
                //Essentially, we will check to see if there's just edge contacts fighting against each other.
                //If they are, then we will correct any stuck-contributing normals to the triangle normal.
                if (vertexContacts.Count == 0 && guaranteedContacts == 0 && edgeContacts.Count > 1)
                {
                    //There are only edge contacts, check to see if:
                    //all normals are coplanar, and
                    //at least one normal faces against the other normals (meaning it's probably stuck, as opposed to just colliding on a corner).

                    bool allNormalsInSamePlane = true;
                    bool atLeastOneNormalAgainst = false;

                    var firstNormal = edgeContacts.Elements[0].ContactData.Normal;
                    edgeContacts.Elements[0].CorrectedNormal.Normalize();
                    float dot;
                    Vector3.Dot(ref firstNormal, ref edgeContacts.Elements[0].CorrectedNormal, out dot);
                    if (Math.Abs(dot) > .01f)
                    {
                        //Go ahead and test the first contact separately, since we're using its contact normal to determine coplanarity.
                        allNormalsInSamePlane = false;
                    }
                    else
                    {
                        //TODO: Note that we're only checking the new edge contacts, not the existing contacts.
                        //It's possible that some existing contacts could interfere and cause issues, but for the sake of simplicity and due to rarity
                        //we'll ignore that possibility for now.
                        for (int i = 1; i < edgeContacts.Count; i++)
                        {
                            Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref firstNormal, out dot);
                            if (dot < 0)
                            {
                                atLeastOneNormalAgainst = true;
                            }
                            //Check to see if the normal is outside the plane.
                            Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref edgeContacts.Elements[0].CorrectedNormal, out dot);

                            if (Math.Abs(dot) > .01f)
                            {

                                //We are not stuck!
                                allNormalsInSamePlane = false;
                                break;
                            }
                        }
                    }

                    if (allNormalsInSamePlane && atLeastOneNormalAgainst)
                    {
                        //Uh oh! all the normals are parallel... The object is probably in a weird situation.
                        //Let's correct the normals!

                        //Already normalized the first contact above.
                        //We don't need to perform the perpendicularity test here- we did that before! We know it's perpendicular already.
                        edgeContacts.Elements[0].ContactData.Normal = edgeContacts.Elements[0].CorrectedNormal;
                        edgeContacts.Elements[0].ShouldCorrect = true;

                        for (int i = 1; i < edgeContacts.Count; i++)
                        {
                            //Must normalize the corrected normal before using it.
                            edgeContacts.Elements[i].CorrectedNormal.Normalize();
                            Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
                            if (dot < .01)
                            {
                                //Only bother doing the correction if the normal appears to be pointing nearly horizontally- implying that it's a contributor to the stuckness!
                                //If it's blocked, the next section will use the corrected normal- if it's not blocked, the next section will use the direct normal.
                                //Make them the same thing :)
                                edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal;
                                edgeContacts.Elements[i].ShouldCorrect = true;
                                //Note that the penetration depth is NOT corrected.  The contact's depth no longer represents the true depth.
                                //However, we only need to have some penetration depth to get the object to escape the rut.
                                //Furthermore, the depth computed from the horizontal opposing contacts is known to be less than the depth in the perpendicular direction.
                                //If the current depth was NOT less than the true depth along the corrected normal, then the collision detection system 
                                //would have picked a different depth, as it finds a reasonable approximation of the minimum penetration!
                                //As a consequence, this contact will not be active beyond the object's destuckification, because its contact depth will be negative (or very close to it).

                            }
                        }
                    }
                }


              


                for (int i = 0; i < edgeContacts.Count; i++)
                {
                    //Only correct if it's allowed AND it's blocked.
                    //If it's not blocked, the contact being created is necessary!
                    //The normal generated by the triangle-convex tester is already known not to
                    //violate the triangle sidedness.
                    if (!blockedEdgeRegions.Contains(edgeContacts.Elements[i].Edge))
                    {
                        //If it's not blocked, use the contact as-is without correcting it.
                        AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);

                    }
                    else if (edgeContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
                    {
                        //If it is blocked, we can still make use of the contact.  But first, we need to change the contact normal to ensure that
                        //it will not interfere (and cause a bump or something).
                        float dot;
                        edgeContacts.Elements[i].CorrectedNormal.Normalize();
                        Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
                        edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal;
                        edgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
                        AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
                    }
                    //If it's blocked AND it doesn't allow correction, ignore its existence.



                }




                for (int i = 0; i < vertexContacts.Count; i++)
                {

                    if (!blockedVertexRegions.Contains(vertexContacts.Elements[i].Vertex))
                    {
                        //If it's not blocked, use the contact as-is without correcting it.
                        AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
                    }
                    else if (vertexContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
                    {
                        //If it is blocked, we can still make use of the contact.  But first, we need to change the contact normal to ensure that
                        //it will not interfere (and cause a bump or something).
                        float dot;
                        vertexContacts.Elements[i].CorrectedNormal.Normalize();
                        Vector3.Dot(ref vertexContacts.Elements[i].CorrectedNormal, ref vertexContacts.Elements[i].ContactData.Normal, out dot);
                        vertexContacts.Elements[i].ContactData.Normal = vertexContacts.Elements[i].CorrectedNormal;
                        vertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
                        AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
                    }
                    //If it's blocked AND it doesn't allow correction, ignore its existence.


                }



                blockedEdgeRegions.Clear();
                blockedVertexRegions.Clear();
                vertexContacts.Clear();
                edgeContacts.Clear();


            }



            //Remove stale pair testers.
            //This will only remove 8 stale ones per frame, but it doesn't really matter.
            //VERY rarely will there be more than 8 in a single frame, and they will be immediately taken care of in the subsequent frame.
            var toRemove = new TinyList<TriangleIndices>();
            foreach (KeyValuePair<TriangleIndices, TrianglePairTester> pair in activePairTesters)
            {
                if (!pair.Value.Updated)
                {
                    if (!toRemove.Add(pair.Key))
                        break;
                }
                else
                    pair.Value.Updated = false;
            }



            for (int i = toRemove.Count - 1; i >= 0; i--)
            {
                var pairTester = activePairTesters[toRemove[i]];
                pairTester.CleanUp();
                GiveBackTester(pairTester);
                activePairTesters.Remove(toRemove[i]);
            }


            //Some child types will want to do some extra post processing on the manifold.        
            ProcessCandidates(candidatesToAdd);


            //Check if adding the new contacts would overflow the manifold.
            if (contacts.Count + candidatesToAdd.Count > 4)
            {
                //Adding all the contacts would overflow the manifold.  Reduce to the best subset.
                ContactReducer.ReduceContacts(contacts, candidatesToAdd, contactIndicesToRemove, reducedCandidates);
                RemoveQueuedContacts();
                for (int i = reducedCandidates.Count - 1; i >= 0; i--)
                {
                    Add(ref reducedCandidates.Elements[i]);
                    reducedCandidates.RemoveAt(i);
                }
            }
            else if (candidatesToAdd.Count > 0)
            {
                //Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
                for (int i = 0; i < candidatesToAdd.Count; i++)
                {
                    Add(ref candidatesToAdd.Elements[i]);
                }
            }



            candidatesToAdd.Clear();

        }

private bool IsContactUnique(ref ContactData contactCandidate)
        {
            contactCandidate.Validate();
            float distanceSquared;
            RigidTransform meshTransform = MeshTransform;
            for (int i = 0; i < contacts.Count; i++)
            {
                Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
                if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
                {
                    //This is a nonconvex manifold.  There will be times where a an object will be shoved into a corner such that
                    //a single position will have two reasonable normals.  If the normals aren't mostly aligned, they should NOT be considered equivalent.
                    Vector3.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
                    if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
                    {
                        //Update the existing 'redundant' contact with the new information.
                        //This works out because the new contact is the deepest contact according to the previous collision detection iteration.
                        contacts.Elements[i].Normal = contactCandidate.Normal;
                        contacts.Elements[i].Position = contactCandidate.Position;
                        contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
                        supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
                        RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA);
                        RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB);
                        return false;
                    }
                }
            }
            for (int i = 0; i < candidatesToAdd.Count; i++)
            {
                Vector3.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
                if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
                {
                    //This is a nonconvex manifold.  There will be times where a an object will be shoved into a corner such that
                    //a single position will have two reasonable normals.  If the normals aren't mostly aligned, they should NOT be considered equivalent.
                    Vector3.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
                    if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
                        return false;
                }
            }
            //for (int i = 0; i < edgeContacts.count; i++)
            //{
            //    Vector3.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
            //    if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
            //    {
            //        return false;
            //    }
            //}
            //for (int i = 0; i < vertexContacts.count; i++)
            //{
            //    Vector3.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
            //    if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
            //    {
            //        return false;
            //    }
            //}
            return true;

        }

public override float SolveIteration()
        {
            float velocityA, velocityB;
            //Find the velocity contribution from each connection
            Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
            Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
            //Add in the constraint space bias velocity
            float lambda = -(velocityA + velocityB) - biasVelocity - usedSoftness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Acreplacedulate the impulse
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Clamp(acreplacedulatedImpulse + lambda, -maxForceDt, maxForceDt);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jacobianA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jacobianB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return Math.Abs(lambda);
        }

public override float SolveIteration()
        {
            float velocityA, velocityB;
            //Find the velocity contribution from each connection
            Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
            Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
            //Add in the constraint space bias velocity
            float lambda = -(velocityA + velocityB) + biasVelocity - usedSoftness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Acreplacedulate the impulse
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Clamp(acreplacedulatedImpulse + lambda, -maxForceDt, maxForceDt);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jacobianA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jacobianB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return Math.Abs(lambda);
        }

public override void Update(float dt)
        {
            basisA.rotationMatrix = connectionA.orientationMatrix;
            basisB.rotationMatrix = connectionB.orientationMatrix;
            basisA.ComputeWorldSpaceAxes();
            basisB.ComputeWorldSpaceAxes();

            if (settings.mode == MotorMode.Servomechanism)
            {
                Quaternion rotation;
                Quaternion.GetQuaternionBetweenNormalizedVectors(ref basisB.primaryAxis, ref basisA.primaryAxis, out rotation);

                //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
                Vector3 twistMeasureAxis;
                Quaternion.Transform(ref basisB.xAxis, ref rotation, out twistMeasureAxis);


                //By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value.
                float y, x;
                Vector3.Dot(ref twistMeasureAxis, ref basisA.yAxis, out y);
                Vector3.Dot(ref twistMeasureAxis, ref basisA.xAxis, out x);
                var angle = (float) Math.Atan2(y, x);

                //Compute goal velocity.
                error = GetDistanceFromGoal(angle);
                float absErrorOverDt = Math.Abs(error / dt);
                float errorReduction;
                settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out usedSoftness);
                biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction;

                biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity);
            }
            else
            {
                biasVelocity = settings.velocityMotor.goalVelocity;
                usedSoftness = settings.velocityMotor.softness / dt;
                error = 0;
            }


            //The nice thing about this approach is that the jacobian entry doesn't flip.
            //Instead, the error can be negative due to the use of Atan2.
            //This is important for limits which have a unique high and low value.

            //Compute the jacobian.
            Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianB);
            if (jacobianB.LengthSquared() < Toolbox.Epsilon)
            {
                //A nasty singularity can show up if the axes are aligned perfectly.
                //In a 'real' situation, this is impossible, so just ignore it.
                isActiveInSolver = false;
                return;
            }

            jacobianB.Normalize();
            jacobianA.X = -jacobianB.X;
            jacobianA.Y = -jacobianB.Y;
            jacobianA.Z = -jacobianB.Z;

            //Update the maximum force
            ComputeMaxForces(settings.maximumForce, dt);


            //****** EFFECTIVE Mreplaced MATRIX ******//
            //Connection A's contribution to the mreplaced matrix
            float entryA;
            Vector3 transformedAxis;
            if (connectionA.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
            }
            else
                entryA = 0;

            //Connection B's contribution to the mreplaced matrix
            float entryB;
            if (connectionB.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
            }
            else
                entryB = 0;

            //Compute the inverse mreplaced matrix
            velocityToImpulse = 1 / (usedSoftness + entryA + entryB);

            
        }

public override float SolveIteration()
        {
            //Compute relative velocity.  Collisions can occur between an enreplacedy and a non-enreplacedy.  If it's not an enreplacedy, replacedume it's not moving.
            float lambda = RelativeVelocity;
            
            lambda *= velocityToImpulse; //convert to impulse

            //Clamp acreplacedulated impulse
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            float maximumFrictionForce = 0;
            for (int i = 0; i < contactCount; i++)
            {
                maximumFrictionForce += leverArms[i] * contactManifoldConstraint.penetrationConstraints.Elements[i].acreplacedulatedImpulse;
            }
            maximumFrictionForce *= friction;
            acreplacedulatedImpulse = MathHelper.Clamp(acreplacedulatedImpulse + lambda, -maximumFrictionForce, maximumFrictionForce); //instead of maximumFrictionForce, could recompute each iteration...
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;


            //Apply the impulse
#if !WINDOWS
            Vector3 angular = new Vector3();
#else
            Vector3 angular;
#endif
            angular.X = lambda * angularX;
            angular.Y = lambda * angularY;
            angular.Z = lambda * angularZ;
            if (enreplacedyADynamic)
            {
                enreplacedyA.ApplyAngularImpulse(ref angular);
            }
            if (enreplacedyBDynamic)
            {
                angular.X = -angular.X;
                angular.Y = -angular.Y;
                angular.Z = -angular.Z;
                enreplacedyB.ApplyAngularImpulse(ref angular);
            }


            return Math.Abs(lambda);
        }

public override void Update(float dt)
        {

            enreplacedyADynamic = enreplacedyA != null && enreplacedyA.isDynamic;
            enreplacedyBDynamic = enreplacedyB != null && enreplacedyB.isDynamic;

            //Compute the jacobian......  Real hard!
            Vector3 normal = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal;
            angularX = normal.X;
            angularY = normal.Y;
            angularZ = normal.Z;

            //Compute inverse effective mreplaced matrix
            float entryA, entryB;

            //these are the transformed coordinates
            float tX, tY, tZ;
            if (enreplacedyADynamic)
            {
                tX = angularX * enreplacedyA.inertiaTensorInverse.M11 + angularY * enreplacedyA.inertiaTensorInverse.M21 + angularZ * enreplacedyA.inertiaTensorInverse.M31;
                tY = angularX * enreplacedyA.inertiaTensorInverse.M12 + angularY * enreplacedyA.inertiaTensorInverse.M22 + angularZ * enreplacedyA.inertiaTensorInverse.M32;
                tZ = angularX * enreplacedyA.inertiaTensorInverse.M13 + angularY * enreplacedyA.inertiaTensorInverse.M23 + angularZ * enreplacedyA.inertiaTensorInverse.M33;
                entryA = tX * angularX + tY * angularY + tZ * angularZ + enreplacedyA.inverseMreplaced;
            }
            else
                entryA = 0;

            if (enreplacedyBDynamic)
            {
                tX = angularX * enreplacedyB.inertiaTensorInverse.M11 + angularY * enreplacedyB.inertiaTensorInverse.M21 + angularZ * enreplacedyB.inertiaTensorInverse.M31;
                tY = angularX * enreplacedyB.inertiaTensorInverse.M12 + angularY * enreplacedyB.inertiaTensorInverse.M22 + angularZ * enreplacedyB.inertiaTensorInverse.M32;
                tZ = angularX * enreplacedyB.inertiaTensorInverse.M13 + angularY * enreplacedyB.inertiaTensorInverse.M23 + angularZ * enreplacedyB.inertiaTensorInverse.M33;
                entryB = tX * angularX + tY * angularY + tZ * angularZ + enreplacedyB.inverseMreplaced;
            }
            else
                entryB = 0;

            velocityToImpulse = -1 / (entryA + entryB);


            //Compute the relative velocity to determine what kind of friction to use
            float relativeAngularVelocity = RelativeVelocity;
            //Set up friction and find maximum friction force
            Vector3 relativeSlidingVelocity = contactManifoldConstraint.SlidingFriction.relativeVelocity;
            friction = Math.Abs(relativeAngularVelocity) > CollisionResponseSettings.StaticFrictionVelocityThreshold ||
                       Math.Abs(relativeSlidingVelocity.X) + Math.Abs(relativeSlidingVelocity.Y) + Math.Abs(relativeSlidingVelocity.Z) > CollisionResponseSettings.StaticFrictionVelocityThreshold
                           ? contactManifoldConstraint.materialInteraction.KineticFriction
                           : contactManifoldConstraint.materialInteraction.StaticFriction;
            friction *= CollisionResponseSettings.TwistFrictionFactor;

            contactCount = contactManifoldConstraint.penetrationConstraints.Count;

            Vector3 contactOffset;
            for (int i = 0; i < contactCount; i++)
            {
                Vector3.Subtract(ref contactManifoldConstraint.penetrationConstraints.Elements[i].contact.Position, ref contactManifoldConstraint.SlidingFriction.manifoldCenter, out contactOffset);
                leverArms[i] = contactOffset.Length();
            }



        }

public override float SolveIteration()
        {
            float velocityA, velocityB;
            //Find the velocity contribution from each connection
            Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
            Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
            //Add in the constraint space bias velocity
            float lambda = (-velocityA - velocityB) - biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Clamp acreplacedulated impulse (can't go negative)
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Min(acreplacedulatedImpulse + lambda, 0);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jacobianA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jacobianB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override float SolveIteration()
        {
            //Compute the current relative velocity.
            float lambda, dot;
            Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda);
            Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
            lambda += dot;

            //Add in the constraint space bias velocity
            lambda = -lambda + biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Acreplacedulate impulse
            acreplacedulatedImpulse += lambda;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jLinearA, lambda, out impulse);
                connectionA.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jLinearB, lambda, out impulse);
                connectionB.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override float SolveIteration()
        {
            float velocityA, velocityB;
            //Find the velocity contribution from each connection
            Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
            Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
            //Add in the constraint space bias velocity
            float lambda = -(velocityA + velocityB) - biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Acreplacedulate the impulse
            acreplacedulatedImpulse += lambda;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jacobianA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jacobianB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override float SolveIteration()
        {
            float velocityA, velocityB;
            //Find the velocity contribution from each connection
            Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
            Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
            //Add in the constraint space bias velocity
            float lambda = -(velocityA + velocityB) + biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Acreplacedulate the impulse
            acreplacedulatedImpulse += lambda;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jacobianA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jacobianB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override float SolveIteration()
        {
            //Compute the current relative velocity.
            float lambda, dot;
            Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda);
            Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
            lambda += dot;

            //Add in the constraint space bias velocity
            lambda = -lambda + biasVelocity - usedSoftness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= mreplacedMatrix;

            //Clamp acreplacedulated impulse
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Clamp(acreplacedulatedImpulse + lambda, -maxForceDt, maxForceDt);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jLinearA, lambda, out impulse);
                connectionA.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jLinearB, lambda, out impulse);
                connectionB.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override void Update(float dt)
        {
            //Compute the 'pre'-jacobians
            Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
            Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);
            Vector3.Add(ref worldOffsetA, ref connectionA.position, out worldAnchorA);
            Vector3.Add(ref worldOffsetB, ref connectionB.position, out worldAnchorB);
            Vector3.Subtract(ref worldAnchorB, ref connectionA.position, out rA);
            Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis);

            float updateRate = 1 / dt;
            if (settings.mode == MotorMode.Servomechanism)
            {
                //Compute error
#if !WINDOWS
            Vector3 separation = new Vector3();
#else
                Vector3 separation;
#endif
                separation.X = worldAnchorB.X - worldAnchorA.X;
                separation.Y = worldAnchorB.Y - worldAnchorA.Y;
                separation.Z = worldAnchorB.Z - worldAnchorA.Z;

                Vector3.Dot(ref separation, ref worldAxis, out error);

                //Compute error
                error = error - settings.servo.goal;


                //Compute bias
                float absErrorOverDt = Math.Abs(error * updateRate);
                float errorReduction;
                settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
                biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction;
                biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity);
            }
            else
            {
                biasVelocity = -settings.velocityMotor.goalVelocity;
                usedSoftness = settings.velocityMotor.softness * updateRate;
                error = 0;
            }

            //Compute jacobians
            jLinearA = worldAxis;
            jLinearB.X = -jLinearA.X;
            jLinearB.Y = -jLinearA.Y;
            jLinearB.Z = -jLinearA.Z;
            Vector3.Cross(ref rA, ref jLinearA, out jAngularA);
            Vector3.Cross(ref worldOffsetB, ref jLinearB, out jAngularB);

            //compute mreplaced matrix
            float entryA, entryB;
            Vector3 intermediate;
            if (connectionA.isDynamic)
            {
                Matrix3x3.Transform(ref jAngularA, ref connectionA.inertiaTensorInverse, out intermediate);
                Vector3.Dot(ref intermediate, ref jAngularA, out entryA);
                entryA += connectionA.inverseMreplaced;
            }
            else
                entryA = 0;
            if (connectionB.isDynamic)
            {
                Matrix3x3.Transform(ref jAngularB, ref connectionB.inertiaTensorInverse, out intermediate);
                Vector3.Dot(ref intermediate, ref jAngularB, out entryB);
                entryB += connectionB.inverseMreplaced;
            }
            else
                entryB = 0;
            mreplacedMatrix = 1 / (entryA + entryB + usedSoftness);

            //Update the maximum force
            ComputeMaxForces(settings.maximumForce, dt);



        }

private static void ComputeInitialTetrahedron(RawList<Vector3> points, RawList<int> outsidePointCandidates, RawList<int> triangleIndices, out Vector3 centroid)
        {
            //Find four points on the hull.
            //We'll start with using the x axis to identify two points on the hull.
            int a, b, c, d;
            Vector3 direction;
            //Find the extreme points along the x axis.
            float minimumX = float.MaxValue, maximumX = -float.MaxValue;
            int minimumXIndex = 0, maximumXIndex = 0;
            for (int i = 0; i < points.Count; ++i)
            {
                var v = points.Elements[i];
                if (v.X > maximumX)
                {
                    maximumX = v.X;
                    maximumXIndex = i;
                }
                else if (v.X < minimumX)
                {
                    minimumX = v.X;
                    minimumXIndex = i;
                }
            }
            a = minimumXIndex;
            b = maximumXIndex;
            //Check for redundancies..
            if (a == b)
                throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");

            //Now, use a second axis perpendicular to the two points we found.
            Vector3 ab;
            Vector3.Subtract(ref points.Elements[b], ref points.Elements[a], out ab);
            Vector3.Cross(ref ab, ref Toolbox.UpVector, out direction);
            if (direction.LengthSquared() < Toolbox.Epsilon)
                Vector3.Cross(ref ab, ref Toolbox.RightVector, out direction);
            float minimumDot, maximumDot;
            int minimumIndex, maximumIndex;
            GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
            //Compare the location of the extreme points to the location of the axis.
            float dot;
            Vector3.Dot(ref direction, ref points.Elements[a], out dot);
            //Use the point further from the axis.
            if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
            {
                //In this case, we should use the minimum index.
                c = minimumIndex;
            }
            else
            {
                //In this case, we should use the maximum index.
                c = maximumIndex;
            }

            //Check for redundancies..
            if (a == c || b == c)
                throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");

            //Use a third axis perpendicular to the plane defined by the three unique points a, b, and c.
            Vector3 ac;
            Vector3.Subtract(ref points.Elements[c], ref points.Elements[a], out ac);
            Vector3.Cross(ref ab, ref ac, out direction);

            GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
            //Compare the location of the extreme points to the location of the plane.
            Vector3.Dot(ref direction, ref points.Elements[a], out dot);
            //Use the point further from the plane. 
            if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
            {
                //In this case, we should use the minimum index.
                d = minimumIndex;
            }
            else
            {
                //In this case, we should use the maximum index.
                d = maximumIndex;
            }

            //Check for redundancies..
            if (a == d || b == d || c == d)
                throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");

            //Add the triangles.
            triangleIndices.Add(a);
            triangleIndices.Add(b);
            triangleIndices.Add(c);

            triangleIndices.Add(a);
            triangleIndices.Add(b);
            triangleIndices.Add(d);

            triangleIndices.Add(a);
            triangleIndices.Add(c);
            triangleIndices.Add(d);

            triangleIndices.Add(b);
            triangleIndices.Add(c);
            triangleIndices.Add(d);

            //The centroid is guaranteed to be within the convex hull.  It will be used to verify the windings of triangles throughout the hull process.
            Vector3.Add(ref points.Elements[a], ref points.Elements[b], out centroid);
            Vector3.Add(ref centroid, ref points.Elements[c], out centroid);
            Vector3.Add(ref centroid, ref points.Elements[d], out centroid);
            Vector3.Multiply(ref centroid, 0.25f, out centroid);

            for (int i = 0; i < triangleIndices.Count; i += 3)
            {
                var vA = points.Elements[triangleIndices.Elements[i]];
                var vB = points.Elements[triangleIndices.Elements[i + 1]];
                var vC = points.Elements[triangleIndices.Elements[i + 2]];

                //Check the signed volume of a parallelepiped with the edges of this triangle and the centroid.
                Vector3 cross;
                Vector3.Subtract(ref vB, ref vA, out ab);
                Vector3.Subtract(ref vC, ref vA, out ac);
                Vector3.Cross(ref ac, ref ab, out cross);
                Vector3 offset;
                Vector3.Subtract(ref vA, ref centroid, out offset);
                float volume;
                Vector3.Dot(ref offset, ref cross, out volume);
                //This volume/cross product could also be used to check for degeneracy, but we already tested for that.
                if (Math.Abs(volume) < Toolbox.BigEpsilon)
                {
                    throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
                }
                if (volume < 0)
                {
                    //If the signed volume is negative, that means the triangle's winding is opposite of what we want.
                    //Flip it around!
                    var temp = triangleIndices.Elements[i];
                    triangleIndices.Elements[i] = triangleIndices.Elements[i + 1];
                    triangleIndices.Elements[i + 1] = temp;
                }
            }

            //Points which belong to the tetrahedra are guaranteed to be 'in' the convex hull. Do not allow them to be considered.
            var tetrahedronIndices = CommonResources.GetIntList();
            tetrahedronIndices.Add(a);
            tetrahedronIndices.Add(b);
            tetrahedronIndices.Add(c);
            tetrahedronIndices.Add(d);
            //Sort the indices to allow a linear time loop.
            Array.Sort(tetrahedronIndices.Elements, 0, 4);
            int tetrahedronIndex = 0;
            for (int i = 0; i < points.Count; ++i)
            {
                if (tetrahedronIndex < 4 && i == tetrahedronIndices[tetrahedronIndex])
                {
                    //Don't add a tetrahedron index. Now that we've found this index, though, move on to the next one.
                    ++tetrahedronIndex;
                }
                else
                {
                    outsidePointCandidates.Add(i);
                }
            }
            CommonResources.GiveBack(tetrahedronIndices);
        }

public Vector4 ToAxisAngle()
        {
            Quaternion q = this;
            if (Math.Abs(q.W) > 1.0f)
                q.Normalize();

            Vector4 result = new Vector4();

            result.W = 2.0f * (float)System.Math.Acos(q.W); // angle
            float den = (float)System.Math.Sqrt(1.0 - q.W * q.W);
            if (den > 0.0001f)
            {
                result.Xyz = q.Xyz / den;
            }
            else
            {
                // This occurs when the angle is zero. 
                // Not a problem: just set an arbitrary normalized axis.
                result.Xyz = Vector3.UnitX;
            }

            return result;
        }

public override float SolveIteration()
        {
            //Compute relative velocity and convert to impulse
            float lambda = RelativeVelocity * velocityToImpulse;


            //Clamp acreplacedulated impulse
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            float maxForce = friction * penetrationConstraint.acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Clamp(acreplacedulatedImpulse + lambda, -maxForce, maxForce);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
#if !WINDOWS
            Vector3 linear = new Vector3();
            Vector3 angular = new Vector3();
#else
            Vector3 linear, angular;
#endif
            linear.X = lambda * linearAX;
            linear.Y = lambda * linearAY;
            linear.Z = lambda * linearAZ;
            if (enreplacedyAIsDynamic)
            {
                angular.X = lambda * angularAX;
                angular.Y = lambda * angularAY;
                angular.Z = lambda * angularAZ;
                enreplacedyA.ApplyLinearImpulse(ref linear);
                enreplacedyA.ApplyAngularImpulse(ref angular);
            }
            if (enreplacedyBIsDynamic)
            {
                linear.X = -linear.X;
                linear.Y = -linear.Y;
                linear.Z = -linear.Z;
                angular.X = lambda * angularBX;
                angular.Y = lambda * angularBY;
                angular.Z = lambda * angularBZ;
                enreplacedyB.ApplyLinearImpulse(ref linear);
                enreplacedyB.ApplyAngularImpulse(ref angular);
            }

            return Math.Abs(lambda);
        }

public override float SolveIteration()
        {

            //Compute relative velocity
            float lambda = (RelativeVelocity - bias + softness * acreplacedulatedImpulse) * velocityToImpulse;

            //Clamp acreplacedulated impulse
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Max(0, acreplacedulatedImpulse + lambda);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;


            //Apply the impulse
#if !WINDOWS
            Vector3 linear = new Vector3();
            Vector3 angular = new Vector3();
#else
            Vector3 linear, angular;
#endif
            linear.X = lambda * linearAX;
            linear.Y = lambda * linearAY;
            linear.Z = lambda * linearAZ;
            if (enreplacedyADynamic)
            {
                angular.X = lambda * angularAX;
                angular.Y = lambda * angularAY;
                angular.Z = lambda * angularAZ;
                enreplacedyA.ApplyLinearImpulse(ref linear);
                enreplacedyA.ApplyAngularImpulse(ref angular);
            }
            if (enreplacedyBDynamic)
            {
                linear.X = -linear.X;
                linear.Y = -linear.Y;
                linear.Z = -linear.Z;
                angular.X = lambda * angularBX;
                angular.Y = lambda * angularBY;
                angular.Z = lambda * angularBZ;
                enreplacedyB.ApplyLinearImpulse(ref linear);
                enreplacedyB.ApplyAngularImpulse(ref angular);
            }

            return Math.Abs(lambda);
        }

public override float SolveIteration()
        {
            //Compute the current relative velocity.
            float lambda, dot;
            Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda);
            Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
            lambda += dot;

            //Add in the constraint space bias velocity
            lambda = -lambda + biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Clamp acreplacedulated impulse (can't go negative)
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Max(acreplacedulatedImpulse + lambda, 0);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jLinearA, lambda, out impulse);
                connectionA.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jLinearB, lambda, out impulse);
                connectionB.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override float SolveIteration()
        {
            //Compute the current relative velocity.
            float lambda, dot;
            Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda);
            Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
            lambda += dot;
            Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
            lambda += dot;

            //Add in the constraint space bias velocity
            lambda = -lambda + biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= mreplacedMatrix;

            //Clamp acreplacedulated impulse (can't go negative)
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            if (unadjustedError < 0)
                acreplacedulatedImpulse = MathHelper.Min(acreplacedulatedImpulse + lambda, 0);
            else
                acreplacedulatedImpulse = MathHelper.Max(acreplacedulatedImpulse + lambda, 0);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jLinearA, lambda, out impulse);
                connectionA.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jLinearB, lambda, out impulse);
                connectionB.ApplyLinearImpulse(ref impulse);
                Vector3.Multiply(ref jAngularB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override float SolveIteration()
        {
            float lambda;
            float lambdaTotal = 0;
            float velocityA, velocityB;
            float previousAcreplacedulatedImpulse;
            if (minIsActive)
            {
                //Find the velocity contribution from each connection
                Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out velocityA);
                Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out velocityB);
                //Add in the constraint space bias velocity
                lambda = -(velocityA + velocityB) + biasVelocity.X - softness * acreplacedulatedImpulse.X;

                //Transform to an impulse
                lambda *= velocityToImpulse.X;

                //Clamp acreplacedulated impulse (can't go negative)
                previousAcreplacedulatedImpulse = acreplacedulatedImpulse.X;
                acreplacedulatedImpulse.X = MathHelper.Max(acreplacedulatedImpulse.X + lambda, 0);
                lambda = acreplacedulatedImpulse.X - previousAcreplacedulatedImpulse;

                //Apply the impulse
                Vector3 impulse;
                if (connectionA.isDynamic)
                {
                    Vector3.Multiply(ref jacobianMinA, lambda, out impulse);
                    connectionA.ApplyAngularImpulse(ref impulse);
                }
                if (connectionB.isDynamic)
                {
                    Vector3.Multiply(ref jacobianMinB, lambda, out impulse);
                    connectionB.ApplyAngularImpulse(ref impulse);
                }

                lambdaTotal += Math.Abs(lambda);
            }
            if (maxIsActive)
            {
                //Find the velocity contribution from each connection
                Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out velocityA);
                Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out velocityB);
                //Add in the constraint space bias velocity
                lambda = -(velocityA + velocityB) + biasVelocity.Y - softness * acreplacedulatedImpulse.Y;

                //Transform to an impulse
                lambda *= velocityToImpulse.Y;

                //Clamp acreplacedulated impulse (can't go negative)
                previousAcreplacedulatedImpulse = acreplacedulatedImpulse.Y;
                acreplacedulatedImpulse.Y = MathHelper.Max(acreplacedulatedImpulse.Y + lambda, 0);
                lambda = acreplacedulatedImpulse.Y - previousAcreplacedulatedImpulse;

                //Apply the impulse
                Vector3 impulse;
                if (connectionA.isDynamic)
                {
                    Vector3.Multiply(ref jacobianMaxA, lambda, out impulse);
                    connectionA.ApplyAngularImpulse(ref impulse);
                }
                if (connectionB.isDynamic)
                {
                    Vector3.Multiply(ref jacobianMaxB, lambda, out impulse);
                    connectionB.ApplyAngularImpulse(ref impulse);
                }

                lambdaTotal += Math.Abs(lambda);
            }
            return lambdaTotal;
        }

public override float SolveIteration()
        {
            float lambda;
            Vector3 relativeVelocity;
            Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity);
            //Transform the velocity to with the jacobian
            Vector3.Dot(ref relativeVelocity, ref hingeAxis, out lambda);
            //Add in the constraint space bias velocity
            lambda = -lambda + biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Clamp acreplacedulated impulse (can't go negative)
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Max(acreplacedulatedImpulse + lambda, 0);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            Vector3.Multiply(ref hingeAxis, lambda, out impulse);
            if (connectionA.isDynamic)
            {
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Negate(ref impulse, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return (Math.Abs(lambda));
        }

public override float SolveIteration()
        {
            float velocityA, velocityB;
            //Find the velocity contribution from each connection
            Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
            Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
            //Add in the constraint space bias velocity
            float lambda = -(velocityA + velocityB) + biasVelocity - softness * acreplacedulatedImpulse;

            //Transform to an impulse
            lambda *= velocityToImpulse;

            //Clamp acreplacedulated impulse (can't go negative)
            float previousAcreplacedulatedImpulse = acreplacedulatedImpulse;
            acreplacedulatedImpulse = MathHelper.Max(acreplacedulatedImpulse + lambda, 0);
            lambda = acreplacedulatedImpulse - previousAcreplacedulatedImpulse;

            //Apply the impulse
            Vector3 impulse;
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref jacobianA, lambda, out impulse);
                connectionA.ApplyAngularImpulse(ref impulse);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Multiply(ref jacobianB, lambda, out impulse);
                connectionB.ApplyAngularImpulse(ref impulse);
            }

            return Math.Abs(lambda);
        }

public override void Update(float dt)
        {
            basisA.rotationMatrix = connectionA.orientationMatrix;
            basisB.rotationMatrix = connectionB.orientationMatrix;
            basisA.ComputeWorldSpaceAxes();
            basisB.ComputeWorldSpaceAxes();

            Quaternion rotation;
            Quaternion.GetQuaternionBetweenNormalizedVectors(ref basisB.primaryAxis, ref basisA.primaryAxis, out rotation);

            //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
            Vector3 twistMeasureAxis;
            Quaternion.Transform(ref basisB.xAxis, ref rotation, out twistMeasureAxis);

            //By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value.
            float y, x;
            Vector3.Dot(ref twistMeasureAxis, ref basisA.yAxis, out y);
            Vector3.Dot(ref twistMeasureAxis, ref basisA.xAxis, out x);
            var angle = (float) Math.Atan2(y, x);

            float distanceFromCurrent, distanceFromMaximum;
            if (IsAngleValid(angle, out distanceFromCurrent, out distanceFromMaximum))
            {
                isActiveInSolver = false;
                acreplacedulatedImpulse = 0;
                error = 0;
                isLimitActive = false;
                return;
            }
            isLimitActive = true;

            //Compute the jacobian.
            if (error > 0)
            {
                Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianB);
                if (jacobianB.LengthSquared() < Toolbox.Epsilon)
                {
                    //A nasty singularity can show up if the axes are aligned perfectly.
                    //In a 'real' situation, this is impossible, so just ignore it.
                    isActiveInSolver = false;
                    return;
                }

                jacobianB.Normalize();
                jacobianA.X = -jacobianB.X;
                jacobianA.Y = -jacobianB.Y;
                jacobianA.Z = -jacobianB.Z;
            }
            else
            {
                //Reverse the jacobian so that the solver loop is easier.
                Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianA);
                if (jacobianA.LengthSquared() < Toolbox.Epsilon)
                {
                    //A nasty singularity can show up if the axes are aligned perfectly.
                    //In a 'real' situation, this is impossible, so just ignore it.
                    isActiveInSolver = false;
                    return;
                }

                jacobianA.Normalize();
                jacobianB.X = -jacobianA.X;
                jacobianB.Y = -jacobianA.Y;
                jacobianB.Z = -jacobianA.Z;
            }

            //****** VELOCITY BIAS ******//
            //Compute the correction velocity.
            error = ComputeAngleError(distanceFromCurrent, distanceFromMaximum);
            float errorReduction;
            springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);


            //biasVelocity = MathHelper.Clamp(-error * myCorrectionStrength / dt, -myMaxCorrectiveVelocity, myMaxCorrectiveVelocity);
            biasVelocity = MathHelper.Min(MathHelper.Max(0, Math.Abs(error) - margin) * errorReduction, maxCorrectiveVelocity);
            if (bounciness > 0)
            {
                float relativeVelocity;
                float dot;
                //Find the velocity contribution from each connection
                Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity);
                Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out dot);
                relativeVelocity += dot;
                biasVelocity = MathHelper.Max(biasVelocity, ComputeBounceVelocity(-relativeVelocity));
            }

            //The nice thing about this approach is that the jacobian entry doesn't flip.
            //Instead, the error can be negative due to the use of Atan2.
            //This is important for limits which have a unique high and low value.


            //****** EFFECTIVE Mreplaced MATRIX ******//
            //Connection A's contribution to the mreplaced matrix
            float entryA;
            Vector3 transformedAxis;
            if (connectionA.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
            }
            else
                entryA = 0;

            //Connection B's contribution to the mreplaced matrix
            float entryB;
            if (connectionB.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
            }
            else
                entryB = 0;

            //Compute the inverse mreplaced matrix
            velocityToImpulse = 1 / (softness + entryA + entryB);

            
        }

public override void Update(float dt)
        {
            //Transform the axes into world space.
            basis.rotationMatrix = connectionA.orientationMatrix;
            basis.ComputeWorldSpaceAxes();
            Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);

            float updateRate = 1 / dt;
            if (settings.mode == MotorMode.Servomechanism)
            {
                float y, x;
                Vector3 yAxis;
                Vector3.Cross(ref basis.primaryAxis, ref basis.xAxis, out yAxis);
                Vector3.Dot(ref worldTestAxis, ref yAxis, out y);
                Vector3.Dot(ref worldTestAxis, ref basis.xAxis, out x);
                var angle = (float)Math.Atan2(y, x);

                //****** VELOCITY BIAS ******//
                //Compute the correction velocity.
                error = GetDistanceFromGoal(angle);


                float absErrorOverDt = Math.Abs(error * updateRate);
                float errorReduction;
                settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
                biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction;

                biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity);
            }
            else
            {
                biasVelocity = settings.velocityMotor.goalVelocity;
                usedSoftness = settings.velocityMotor.softness * updateRate;
                error = 0;
            }


            //Compute the jacobians
            jacobianA = basis.primaryAxis;
            jacobianB.X = -jacobianA.X;
            jacobianB.Y = -jacobianA.Y;
            jacobianB.Z = -jacobianA.Z;


            //****** EFFECTIVE Mreplaced MATRIX ******//
            //Connection A's contribution to the mreplaced matrix
            float entryA;
            Vector3 transformedAxis;
            if (connectionA.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
            }
            else
                entryA = 0;

            //Connection B's contribution to the mreplaced matrix
            float entryB;
            if (connectionB.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
            }
            else
                entryB = 0;

            //Compute the inverse mreplaced matrix
            velocityToImpulse = 1 / (usedSoftness + entryA + entryB);


            //Update the maximum force
            ComputeMaxForces(settings.maximumForce, dt);



        }

internal void PreStep(float dt)
        {
            vehicleEnreplacedy = wheel.Vehicle.Body;
            supportEnreplacedy = wheel.SupportingEnreplacedy;
            supportIsDynamic = supportEnreplacedy != null && supportEnreplacedy.isDynamic;

            //Grab jacobian and mreplaced matrix from the driving motor!
            linearAX = wheel.drivingMotor.linearAX;
            linearAY = wheel.drivingMotor.linearAY;
            linearAZ = wheel.drivingMotor.linearAZ;

            angularAX = wheel.drivingMotor.angularAX;
            angularAY = wheel.drivingMotor.angularAY;
            angularAZ = wheel.drivingMotor.angularAZ;
            angularBX = wheel.drivingMotor.angularBX;
            angularBY = wheel.drivingMotor.angularBY;
            angularBZ = wheel.drivingMotor.angularBZ;

            velocityToImpulse = wheel.drivingMotor.velocityToImpulse;

            //Friction
            //Which coefficient? Check velocity.
            if (isBraking)
                if (Math.Abs(RelativeVelocity) < staticFrictionVelocityThreshold)
                    blendedCoefficient = frictionBlender(staticBrakingFrictionCoefficient, wheel.supportMaterial.staticFriction, false, wheel);
                else
                    blendedCoefficient = frictionBlender(kineticBrakingFrictionCoefficient, wheel.supportMaterial.kineticFriction, true, wheel);
            else
                blendedCoefficient = rollingFrictionCoefficient;


        }