System.Math.Atan2(double, double)

public Dictionary<Fixture, Vector2> Activate(Vector2 pos, float radius, float maxForce)
        {
            AABB aabb;
            aabb.LowerBound = pos + new Vector2(-radius, -radius);
            aabb.UpperBound = pos + new Vector2(radius, radius);
            Fixture[] shapes = new Fixture[MaxShapes];

            // More than 5 shapes in an explosion could be possible, but still strange.
            Fixture[] containedShapes = new Fixture[5];
            bool exit = false;

            int shapeCount = 0;
            int containedShapeCount = 0;

            // Query the world for overlapping shapes.
            World.QueryAABB(
                fixture =>
                {
                    if (fixture.TestPoint(ref pos))
                    {
                        if (IgnoreWhenInsideShape)
                        {
                            exit = true;
                            return false;
                        }

                        containedShapes[containedShapeCount++] = fixture;
                    }
                    else
                    {
                        shapes[shapeCount++] = fixture;
                    }

                    // Continue the query.
                    return true;
                }, ref aabb);

            if (exit)
                return new Dictionary<Fixture, Vector2>();

            Dictionary<Fixture, Vector2> exploded = new Dictionary<Fixture, Vector2>(shapeCount + containedShapeCount);

            // Per shape max/min angles for now.
            float[] vals = new float[shapeCount * 2];
            int valIndex = 0;
            for (int i = 0; i < shapeCount; ++i)
            {
                PolygonShape ps;
                CircleShape cs = shapes[i].Shape as CircleShape;
                if (cs != null)
                {
                    // We create a "diamond" approximation of the circle
                    Vertices v = new Vertices();
                    Vector2 vec = Vector2.Zero + new Vector2(cs.Radius, 0);
                    v.Add(vec);
                    vec = Vector2.Zero + new Vector2(0, cs.Radius);
                    v.Add(vec);
                    vec = Vector2.Zero + new Vector2(-cs.Radius, cs.Radius);
                    v.Add(vec);
                    vec = Vector2.Zero + new Vector2(0, -cs.Radius);
                    v.Add(vec);
                    ps = new PolygonShape(v, 0);
                }
                else
                    ps = shapes[i].Shape as PolygonShape;

                if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
                {
                    Vector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MreplacedData.Centroid) - pos;
                    float angleToCentroid = (float)Math.Atan2(toCentroid.Y, toCentroid.X);
                    float min = float.MaxValue;
                    float max = float.MinValue;
                    float minAbsolute = 0.0f;
                    float maxAbsolute = 0.0f;

                    for (int j = 0; j < ps.Vertices.Count; ++j)
                    {
                        Vector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
                        float newAngle = (float)Math.Atan2(toVertex.Y, toVertex.X);
                        float diff = (newAngle - angleToCentroid);

                        diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi);
                        // the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be

                        if (diff < 0.0f)
                            diff += 2 * MathHelper.Pi; // correction for not handling negs

                        diff -= MathHelper.Pi;

                        if (Math.Abs(diff) > MathHelper.Pi)
                            continue; // Something's wrong, point not in shape but exists angle diff > 180

                        if (diff > max)
                        {
                            max = diff;
                            maxAbsolute = newAngle;
                        }
                        if (diff < min)
                        {
                            min = diff;
                            minAbsolute = newAngle;
                        }
                    }

                    vals[valIndex] = minAbsolute;
                    ++valIndex;
                    vals[valIndex] = maxAbsolute;
                    ++valIndex;
                }
            }

            Array.Sort(vals, 0, valIndex, _rdc);
            _data.Clear();
            bool rayMissed = true;

            for (int i = 0; i < valIndex; ++i)
            {
                Fixture fixture = null;
                float midpt;

                int iplus = (i == valIndex - 1 ? 0 : i + 1);
                if (vals[i] == vals[iplus])
                    continue;

                if (i == valIndex - 1)
                {
                    // the single edgecase
                    midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]);
                }
                else
                {
                    midpt = (vals[i + 1] + vals[i]);
                }

                midpt = midpt / 2;

                Vector2 p1 = pos;
                Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt), (float)Math.Sin(midpt)) + pos;

                // RaycastOne
                bool hitClosest = false;
                World.RayCast((f, p, n, fr) =>
                                  {
                                      Body body = f.Body;

                                      if (!IsActiveOn(body))
                                          return 0;

                                      hitClosest = true;
                                      fixture = f;
                                      return fr;
                                  }, p1, p2);

                //draws radius points
                if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic))
                {
                    if ((_data.Any()) && (_data.Last().Body == fixture.Body) && (!rayMissed))
                    {
                        int laPos = _data.Count - 1;
                        ShapeData la = _data[laPos];
                        la.Max = vals[iplus];
                        _data[laPos] = la;
                    }
                    else
                    {
                        // make new
                        ShapeData d;
                        d.Body = fixture.Body;
                        d.Min = vals[i];
                        d.Max = vals[iplus];
                        _data.Add(d);
                    }

                    if ((_data.Count > 1)
                        && (i == valIndex - 1)
                        && (_data.Last().Body == _data.First().Body)
                        && (_data.Last().Max == _data.First().Min))
                    {
                        ShapeData fi = _data[0];
                        fi.Min = _data.Last().Min;
                        _data.RemoveAt(_data.Count - 1);
                        _data[0] = fi;
                        while (_data.First().Min >= _data.First().Max)
                        {
                            fi.Min -= MathHelper.Pi * 2;
                            _data[0] = fi;
                        }
                    }

                    int lastPos = _data.Count - 1;
                    ShapeData last = _data[lastPos];
                    while ((_data.Count > 0)
                           && (_data.Last().Min >= _data.Last().Max)) // just making sure min<max
                    {
                        last.Min = _data.Last().Min - 2 * MathHelper.Pi;
                        _data[lastPos] = last;
                    }
                    rayMissed = false;
                }
                else
                {
                    rayMissed = true; // raycast did not find a shape
                }
            }

            for (int i = 0; i < _data.Count; ++i)
            {
                if (!IsActiveOn(_data[i].Body))
                    continue;

                float arclen = _data[i].Max - _data[i].Min;

                float first = MathHelper.Min(MaxEdgeOffset, EdgeRatio * arclen);
                int insertedRays = (int)Math.Ceiling(((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle);

                if (insertedRays < 0)
                    insertedRays = 0;

                float offset = (arclen - first * 2.0f) / ((float)MinRays + insertedRays - 1);

                //Note: This loop can go into infinite as it operates on floats.
                //Added FloatEquals with a large epsilon.
                for (float j = _data[i].Min + first;
                     j < _data[i].Max || MathUtils.FloatEquals(j, _data[i].Max, 0.0001f);
                     j += offset)
                {
                    Vector2 p1 = pos;
                    Vector2 p2 = pos + radius * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
                    Vector2 hitpoint = Vector2.Zero;
                    float minlambda = float.MaxValue;

                    List<Fixture> fl = _data[i].Body.FixtureList;
                    for (int x = 0; x < fl.Count; x++)
                    {
                        Fixture f = fl[x];
                        RayCastInput ri;
                        ri.Point1 = p1;
                        ri.Point2 = p2;
                        ri.MaxFraction = 50f;

                        RayCastOutput ro;
                        if (f.RayCast(out ro, ref ri, 0))
                        {
                            if (minlambda > ro.Fraction)
                            {
                                minlambda = ro.Fraction;
                                hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
                            }
                        }

                        // the force that is to be applied for this particular ray.
                        // offset is angular coverage. lambda*length of segment is distance.
                        float impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi * (1.0f - Math.Min(1.0f, minlambda));

                        // We Apply the impulse!!!
                        Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j), (float)Math.Sin(j)), -ro.Normal) * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
                        _data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);

                        // We gather the fixtures for returning them
                        if (exploded.ContainsKey(f))
                            exploded[f] += vectImp;
                        else
                            exploded.Add(f, vectImp);

                        if (minlambda > 1.0f)
                            hitpoint = p2;
                    }
                }
            }

            // We check contained shapes
            for (int i = 0; i < containedShapeCount; ++i)
            {
                Fixture fix = containedShapes[i];

                if (!IsActiveOn(fix.Body))
                    continue;

                float impulse = MinRays * maxForce * 180.0f / MathHelper.Pi;
                Vector2 hitPoint;

                CircleShape circShape = fix.Shape as CircleShape;
                if (circShape != null)
                {
                    hitPoint = fix.Body.GetWorldPoint(circShape.Position);
                }
                else
                {
                    PolygonShape shape = fix.Shape as PolygonShape;
                    hitPoint = fix.Body.GetWorldPoint(shape.MreplacedData.Centroid);
                }

                Vector2 vectImp = impulse * (hitPoint - pos);

                fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);

                if (!exploded.ContainsKey(fix))
                    exploded.Add(fix, vectImp);
            }

            return exploded;
        }

IEnumerator PollLocationRoutine()
		{
#if UNITY_EDITOR
			while (!UnityEditor.EditorApplication.isRemoteConnected)
			{
				// exit if we are not the selected location provider
				if (null != LocationProviderFactory.Instance && null != LocationProviderFactory.Instance.DefaultLocationProvider)
				{
					if (!this.Equals(LocationProviderFactory.Instance.DefaultLocationProvider))
					{
						yield break;
					}
				}

				Debug.LogWarning("Remote device not connected via 'Unity Remote'. Waiting ..." + Environment.NewLine + "If Unity seems to be stuck here make sure 'Unity Remote' is running and restart Unity with your device already connected.");
				yield return _wait1sec;
			}
#endif


			//request runtime fine location permission on Android if not yet allowed
#if UNITY_ANDROID
			if (!_locationService.isEnabledByUser)
			{
				UniAndroidPermission.RequestPermission(AndroidPermission.ACCESS_FINE_LOCATION);
				//wait for user to allow or deny
				while (!_gotPermissionRequestResponse) { yield return _wait1sec; }
			}
#endif


			if (!_locationService.isEnabledByUser)
			{
				Debug.LogError("DeviceLocationProvider: Location is not enabled by user!");
				_currentLocation.IsLocationServiceEnabled = false;
				SendLocation(_currentLocation);
				yield break;
			}


			_currentLocation.IsLocationServiceInitializing = true;
			_locationService.Start(_desiredAccuracyInMeters, _updateDistanceInMeters);
			Input.compreplaced.enabled = true;

			int maxWait = 20;
			while (_locationService.status == LocationServiceStatus.Initializing && maxWait > 0)
			{
				yield return _wait1sec;
				maxWait--;
			}

			if (maxWait < 1)
			{
				Debug.LogError("DeviceLocationProvider: " + "Timed out trying to initialize location services!");
				_currentLocation.IsLocationServiceInitializing = false;
				_currentLocation.IsLocationServiceEnabled = false;
				SendLocation(_currentLocation);
				yield break;
			}

			if (_locationService.status == LocationServiceStatus.Failed)
			{
				Debug.LogError("DeviceLocationProvider: " + "Failed to initialize location services!");
				_currentLocation.IsLocationServiceInitializing = false;
				_currentLocation.IsLocationServiceEnabled = false;
				SendLocation(_currentLocation);
				yield break;
			}

			_currentLocation.IsLocationServiceInitializing = false;
			_currentLocation.IsLocationServiceEnabled = true;

#if UNITY_EDITOR
			// HACK: this is to prevent Android devices, connected through Unity Remote, 
			// from reporting a location of (0, 0), initially.
			yield return _wait1sec;
#endif

			System.Globalization.CultureInfo invariantCulture = System.Globalization.CultureInfo.InvariantCulture;

			while (true)
			{

				var lastData = _locationService.lastData;
				var timestamp = lastData.timestamp;

				///////////////////////////////
				// oh boy, Unity what are you doing???
				// on some devices it seems that
				// Input.location.status != LocationServiceStatus.Running
				// nevertheless new location is available
				//////////////////////////////
				//Debug.LogFormat("Input.location.status: {0}", Input.location.status);
				_currentLocation.IsLocationServiceEnabled =
					_locationService.status == LocationServiceStatus.Running
					|| timestamp > _lastLocationTimestamp;

				_currentLocation.IsUserHeadingUpdated = false;
				_currentLocation.IsLocationUpdated = false;

				if (!_currentLocation.IsLocationServiceEnabled)
				{
					yield return _waitUpdateTime;
					continue;
				}

				// device orientation, user heading get calculated below
				_deviceOrientationSmoothing.Add(Input.compreplaced.trueHeading);
				_currentLocation.DeviceOrientation = (float)_deviceOrientationSmoothing.Calculate();


				//_currentLocation.LareplacedudeLongitude = new Vector2d(lastData.lareplacedude, lastData.longitude);
				// HACK to get back to double precision, does this even work?
				// https://forum.unity.com/threads/precision-of-location-longitude-is-worse-when-longitude-is-beyond-100-degrees.133192/#post-1835164
				double lareplacedude = double.Parse(lastData.lareplacedude.ToString("R", invariantCulture), invariantCulture);
				double longitude = double.Parse(lastData.longitude.ToString("R", invariantCulture), invariantCulture);
				Vector2d previousLocation = new Vector2d(_currentLocation.LareplacedudeLongitude.x, _currentLocation.LareplacedudeLongitude.y);
				_currentLocation.LareplacedudeLongitude = new Vector2d(lareplacedude, longitude);

				_currentLocation.Accuracy = (float)Math.Floor(lastData.horizontalAccuracy);
				// sometimes Unity's timestamp doesn't seem to get updated, or even jump back in time
				// do an additional check if location has changed
				_currentLocation.IsLocationUpdated = timestamp > _lastLocationTimestamp || !_currentLocation.LareplacedudeLongitude.Equals(previousLocation);
				_currentLocation.Timestamp = timestamp;
				_lastLocationTimestamp = timestamp;

				if (_currentLocation.IsLocationUpdated)
				{
					if (_lastPositions.Count > 0)
					{
						// only add position if user has moved +1m since we added the previous position to the list
						CheapRuler cheapRuler = new CheapRuler(_currentLocation.LareplacedudeLongitude.x, CheapRulerUnits.Meters);
						Vector2d p = _currentLocation.LareplacedudeLongitude;
						double distance = cheapRuler.Distance(
							new double[] { p.y, p.x },
							new double[] { _lastPositions[0].y, _lastPositions[0].x }
						);
						if (distance > 1.0)
						{
							_lastPositions.Add(_currentLocation.LareplacedudeLongitude);
						}
					}
					else
					{
						_lastPositions.Add(_currentLocation.LareplacedudeLongitude);
					}
				}

				// if we have enough positions calculate user heading ourselves.
				// Unity does not provide bearing based on GPS locations, just
				// device orientation based on Compreplaced.Heading.
				// nevertheless, use compreplaced for intial UserHeading till we have
				// enough values to calculate ourselves.
				if (_lastPositions.Count < _maxLastPositions)
				{
					_currentLocation.UserHeading = _currentLocation.DeviceOrientation;
					_currentLocation.IsUserHeadingUpdated = true;
				}
				else
				{
					Vector2d newestPos = _lastPositions[0];
					Vector2d oldestPos = _lastPositions[_maxLastPositions - 1];
					CheapRuler cheapRuler = new CheapRuler(newestPos.x, CheapRulerUnits.Meters);
					// distance between last and first position in our buffer
					double distance = cheapRuler.Distance(
						new double[] { newestPos.y, newestPos.x },
						new double[] { oldestPos.y, oldestPos.x }
					);
					// positions are minimum required distance apart (user is moving), calculate user heading
					if (distance >= _minDistanceOldestNewestPosition)
					{
						float[] lastHeadings = new float[_maxLastPositions - 1];

						for (int i = 1; i < _maxLastPositions; i++)
						{
							// atan2 increases angle CCW, flip sign of latDiff to get CW
							double latDiff = -(_lastPositions[i].x - _lastPositions[i - 1].x);
							double lngDiff = _lastPositions[i].y - _lastPositions[i - 1].y;
							// +90.0 to make top (north) 0�
							double heading = (Math.Atan2(latDiff, lngDiff) * 180.0 / Math.PI) + 90.0f;
							// stay within [0..360]� range
							if (heading < 0) { heading += 360; }
							if (heading >= 360) { heading -= 360; }
							lastHeadings[i - 1] = (float)heading;
						}

						_userHeadingSmoothing.Add(lastHeadings[0]);
						float finalHeading = (float)_userHeadingSmoothing.Calculate();

						//fix heading to have 0� for north, 90� for east, 180� for south and 270� for west
						finalHeading = finalHeading >= 180.0f ? finalHeading - 180.0f : finalHeading + 180.0f;


						_currentLocation.UserHeading = finalHeading;
						_currentLocation.IsUserHeadingUpdated = true;
					}
				}

				_currentLocation.TimestampDevice = UnixTimestampUtils.To(DateTime.UtcNow);
				SendLocation(_currentLocation);

				yield return _waitUpdateTime;
			}
		}

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;
                }
            }
        }

private float Angle(Point p)
        {
            Point a = (p.Next - p);
            Point b = (p.Prev - p);
            return (float)Math.Atan2(a.Cross(b), a.Dot(b));
        }

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);



        }

public static double CalAngle(Point center, Point p) => Math.Atan2(p.Y - center.Y, p.X - center.X) * 180 / Math.PI;

[System.Diagnostics.Conditional("UNITY_EDITOR")]
		protected void debugLogAngle(double raw, double smoothed)
		{
			double debugAngle = Math.Atan2(Math.Sin(smoothed * DEG2RAD), Math.Cos(smoothed * DEG2RAD)) * RAD2DEG;
			debugAngle = debugAngle < 0 ? debugAngle + 360 : debugAngle >= 360 ? debugAngle - 360 : debugAngle;
			Debug.Log(string.Format("{0:0.000} => {1:0.000}", raw, smoothed));
		}

public float heading(Position position)
        {
            var dir = GetOffset(position);
            dir.Z = 0.0f;

            if (Vec.NormalizeCheckSmall(ref dir))
                return 0.0f;

            return (450.0f - ((float)Math.Atan2(dir.Y, dir.X)).ToDegrees()) % 360.0f;
        }

public override DataPoint InverseTransform(double x, double y, Axis yaxis)
        {
            var angleAxis = yaxis as AngleAxis;
            if (angleAxis == null)
            {
                throw new InvalidOperationException("Polar angle axis not defined!");
            }

            x -= this.MidPoint.x;
            y -= this.MidPoint.y;
            double th = Math.Atan2(y, x);
            double r = Math.Sqrt((x * x) + (y * y));
            x = (r / this.scale) + this.offset;
            y = (th / angleAxis.Scale) + angleAxis.Offset;
            return new DataPoint(x, y);
        }

public static int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target, float gravity, out Vector3 s0, out Vector3 s1, out float t0, out float t1)
        {
            // Handling these cases is up to your project's coding standards
            //Debug.replacedert(proj_pos != target && proj_speed > 0 && gravity > 0, "fts.solve_ballistic_arc called with invalid data");

            // C# requires out variables be set
            s0 = Vector3.Zero;
            s1 = Vector3.Zero;
            t0 = float.PositiveInfinity;
            t1 = float.PositiveInfinity;

            if (proj_pos == target || proj_speed <= 0 || gravity <= 0)
                return 0;

            // Derivation
            //   (1) x = v*t*cos O
            //   (2) z = v*t*sin O - .5*g*t^2
            // 
            //   (3) t = x/(cos O*v)                                        [solve t from (1)]
            //   (4) z = v*x*sin O/(cos O * v) - .5*g*x^2/(cos^2 O*v^2)     [plug t into z=...]
            //   (5) z = x*tan O - g*x^2/(2*v^2*cos^2 O)                    [reduce; cos/sin = tan]
            //   (6) z = x*tan O - (g*x^2/(2*v^2))*(1+tan^2 O)              [reduce; 1+tan O = 1/cos^2 O]
            //   (7) 0 = ((-g*x^2)/(2*v^2))*tan^2 O + x*tan O - (g*x^2)/(2*v^2) - z    [re-arrange]
            //   Quadratic! a*p^2 + b*p + c where p = tan O
            //
            //   (8) let gxv = -g*x*x/(2*v*v)
            //   (9) p = (-x +- sqrt(x*x - 4gxv*(gxv - z)))/2*gxv           [quadratic formula]
            //   (10) p = (v^2 +- sqrt(v^4 - g(g*x^2 + 2*z*v^2)))/gx        [multiply top/bottom by -2*v*v/x; move 4*v^4/x^2 into root]
            //   (11) O = atan(p)

            Vector3 diff = target - proj_pos;
            Vector3 diffXY = new Vector3(diff.X, diff.Y, 0);
            float groundDist = diffXY.Length();

            float speed2 = proj_speed * proj_speed;
            float speed4 = proj_speed * proj_speed * proj_speed * proj_speed;
            float z = diff.Z;
            float x = groundDist;
            float gx = gravity * x;

            float root = speed4 - gravity * (gravity * x * x + 2 * z * speed2);

            // No solution
            if (root < 0)
                return 0;

            root = (float)Math.Sqrt(root);

            var lowAng = Math.Atan2(speed2 - root, gx);
            var highAng = Math.Atan2(speed2 + root, gx);
            int numSolutions = lowAng != highAng ? 2 : 1;

            Vector3 groundDir = Vector3.Normalize(diffXY);
            s0 = groundDir * (float)Math.Cos(lowAng) * proj_speed + Vector3.UnitZ * (float)Math.Sin(lowAng) * proj_speed;
            if (numSolutions > 1)
                s1 = groundDir * (float)Math.Cos(highAng) * proj_speed + Vector3.UnitZ * (float)Math.Sin(highAng) * proj_speed;

            t0 = x / ((float)Math.Cos(lowAng) * proj_speed);
            t1 = x / ((float)Math.Cos(highAng) * proj_speed);

            return numSolutions;
        }

private static double HoleAngle(AdvancingFrontNode node)
        {
            // XXX: do we really need a signed angle for holeAngle?
            //      could possible save some cycles here
            /* Complex plane
             * ab = cosA +i*sinA
             * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx)
             * atan2(y,x) computes the principal value of the argument function
             * applied to the complex number x+iy
             * Where x = ax*bx + ay*by
             *       y = ax*by - ay*bx
             */
            double px = node.Point.X;
            double py = node.Point.Y;
            double ax = node.Next.Point.X - px;
            double ay = node.Next.Point.Y - py;
            double bx = node.Prev.Point.X - px;
            double by = node.Prev.Point.Y - py;
            return Math.Atan2(ax * by - ay * bx, ax * bx + ay * by);
        }

[ContextMethod("АТангенс2", "Atan2")]
        public double Atan2(double p1, double p2)
        {
           return System.Math.Atan2(p1, p2);
        }

public static int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target, float gravity, out Vector3 s0, out Vector3 s1, out float t0, out float t1)
        {

            // Handling these cases is up to your project's coding standards
            Debug.replacedert(proj_pos != target && proj_speed > 0 && gravity > 0, "fts.solve_ballistic_arc called with invalid data");

            // C# requires out variables be set
            s0 = Vector3.Zero;
            s1 = Vector3.Zero;
            t0 = float.PositiveInfinity;
            t1 = float.PositiveInfinity;

            // Derivation
            //   (1) x = v*t*cos O
            //   (2) z = v*t*sin O - .5*g*t^2
            // 
            //   (3) t = x/(cos O*v)                                        [solve t from (1)]
            //   (4) z = v*x*sin O/(cos O * v) - .5*g*x^2/(cos^2 O*v^2)     [plug t into z=...]
            //   (5) z = x*tan O - g*x^2/(2*v^2*cos^2 O)                    [reduce; cos/sin = tan]
            //   (6) z = x*tan O - (g*x^2/(2*v^2))*(1+tan^2 O)              [reduce; 1+tan O = 1/cos^2 O]
            //   (7) 0 = ((-g*x^2)/(2*v^2))*tan^2 O + x*tan O - (g*x^2)/(2*v^2) - z    [re-arrange]
            //   Quadratic! a*p^2 + b*p + c where p = tan O
            //
            //   (8) let gxv = -g*x*x/(2*v*v)
            //   (9) p = (-x +- sqrt(x*x - 4gxv*(gxv - z)))/2*gxv           [quadratic formula]
            //   (10) p = (v^2 +- sqrt(v^4 - g(g*x^2 + 2*z*v^2)))/gx        [multiply top/bottom by -2*v*v/x; move 4*v^4/x^2 into root]
            //   (11) O = atan(p)

            Vector3 diff = target - proj_pos;
            Vector3 diffXY = new Vector3(diff.X, diff.Y, 0);
            float groundDist = diffXY.Length();

            float speed2 = proj_speed * proj_speed;
            float speed4 = proj_speed * proj_speed * proj_speed * proj_speed;
            float z = diff.Z;
            float x = groundDist;
            float gx = gravity * x;

            float root = speed4 - gravity * (gravity * x * x + 2 * z * speed2);

            // No solution
            if (root < 0)
                return 0;

            root = (float)Math.Sqrt(root);

            var lowAng = Math.Atan2(speed2 - root, gx);
            var highAng = Math.Atan2(speed2 + root, gx);
            int numSolutions = lowAng != highAng ? 2 : 1;

            Vector3 groundDir = diffXY.Normalize();
            s0 = groundDir * (float)Math.Cos(lowAng) * proj_speed + Vector3.UnitZ * (float)Math.Sin(lowAng) * proj_speed;
            if (numSolutions > 1)
                s1 = groundDir * (float)Math.Cos(highAng) * proj_speed + Vector3.UnitZ * (float)Math.Sin(highAng) * proj_speed;

            t0 = x / ((float)Math.Cos(lowAng) * proj_speed);
            t1 = x / ((float)Math.Cos(highAng) * proj_speed);

            return numSolutions;
        }

public static double VectorAngle(ref Vector2 p1, ref Vector2 p2)
        {
            double theta1 = Math.Atan2(p1.Y, p1.X);
            double theta2 = Math.Atan2(p2.Y, p2.X);
            double dtheta = theta2 - theta1;
            while (dtheta > Math.PI)
                dtheta -= (2 * Math.PI);
            while (dtheta < -Math.PI)
                dtheta += (2 * Math.PI);

            return (dtheta);
        }

public float get_heading()
        {
            var matrix = Matrix4x4.CreateFromQuaternion(Orientation);
            var heading = (float)Math.Atan2(matrix.M22, matrix.M21);
            return (450.0f - heading.ToDegrees()) % 360.0f;
        }

public List<Vector3> SubdivideEvenly(int divisions)
        {
            List<Vector3> verts = new List<Vector3>();

            float length = GetLength();

            float deltaLength = length / divisions + 0.001f;
            float t = 0.000f;

            // we always start at the first control point
            Vector2 start = ControlPoints[0];
            Vector2 end = GetPosition(t);

            // increment t until we are at half the distance
            while (deltaLength * 0.5f >= Vector2.Distance(start, end))
            {
                end = GetPosition(t);
                t += 0.0001f;

                if (t >= 1f)
                    break;
            }

            start = end;

            // for each box
            for (int i = 1; i < divisions; i++)
            {
                Vector2 normal = GetPositionNormal(t);
                float angle = (float)Math.Atan2(normal.Y, normal.X);

                verts.Add(new Vector3(end, angle));

                // until we reach the correct distance down the curve
                while (deltaLength >= Vector2.Distance(start, end))
                {
                    end = GetPosition(t);
                    t += 0.00001f;

                    if (t >= 1f)
                        break;
                }
                if (t >= 1f)
                    break;

                start = end;
            }
            return verts;
        }

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;
                }
            }
        }

public override double Calculate()
		{
			// reverse order, _angles[0] is latest
			double[] angles = _angles.Reverse().ToArray();

			// since we cannot work directly on the angles (eg think about 355 and 5)
			// we convert to cartesian coordinates and apply filtering there
			// aproximation should be good enough for the use case of compreplaced filtering
			// differences occur only at the 2nd or 3rd digit after the decimal point

			double sin = Math.Sin(angles[0] * DEG2RAD);
			double cos = Math.Cos(angles[0] * DEG2RAD);
			debugLogAngle(angles[0], Math.Atan2(sin, cos) * RAD2DEG);

			for (int i = 1; i < angles.Length; i++)
			{
				sin = (Math.Sin(angles[i] * DEG2RAD) - sin) * _alpha + sin;
				cos = (Math.Cos(angles[i] * DEG2RAD) - cos) * _alpha + cos;
				debugLogAngle(angles[i], Math.Atan2(sin, cos) * RAD2DEG);
			}

			// round, don't need crazy precision
			double finalAngle = Math.Round(Math.Atan2(sin, cos) * RAD2DEG, 2);
			debugLogAngle(finalAngle, finalAngle);
			// stay within [0..<360]
			finalAngle = finalAngle < 0 ? finalAngle + 360 : finalAngle >= 360 ? finalAngle - 360 : finalAngle;
			debugLogAngle(finalAngle, finalAngle);

			return finalAngle;
		}

public void CalculateDots(double x, double y, double timeInMs)
        {
            var outVelocity = AccelCalculator.Velocity(x, y, timeInMs);
            var outAngle = Math.Atan2(Math.Abs(y),Math.Abs(x));
            var nearestAngleDivision = AccelCalculator.NearestAngleDivision(outAngle);
            var data = AngleToData[nearestAngleDivision];
            var index = data.GetVelocityIndex(outVelocity);
            var inVelocity = data.VelocityPoints.ElementAt(index).Key;
            var xPoints = X.ValuesAtIndex(index);
            var yPoints = Y.ValuesAtIndex(index);
            XPoints.Sensitivity.Set(inVelocity, xPoints.Item1);
            XPoints.Velocity.Set(inVelocity, xPoints.Item2);
            XPoints.Gain.Set(inVelocity, xPoints.Item3);
            YPoints.Sensitivity.Set(inVelocity, yPoints.Item1);
            YPoints.Velocity.Set(inVelocity, yPoints.Item2);
            YPoints.Gain.Set(inVelocity, yPoints.Item3);
        }

private static bool GetPointAtRelativeDistance(
            IList<ScreenPoint> pts, double p, double margin, out ScreenPoint position, out double angle)
        {
            if (pts == null || pts.Count == 0)
            {
                position = new ScreenPoint();
                angle = 0;
                return false;
            }

            double length = 0;
            for (int i = 1; i < pts.Count; i++)
            {
                length += (pts[i] - pts[i - 1]).Length;
            }

            double l = (length * p) + margin;
            length = 0;
            for (int i = 1; i < pts.Count; i++)
            {
                double dl = (pts[i] - pts[i - 1]).Length;
                if (l >= length && l <= length + dl)
                {
                    double f = (l - length) / dl;
                    double x = (pts[i].X * f) + (pts[i - 1].X * (1 - f));
                    double y = (pts[i].Y * f) + (pts[i - 1].Y * (1 - f));
                    position = new ScreenPoint(x, y);
                    double dx = pts[i].X - pts[i - 1].X;
                    double dy = pts[i].Y - pts[i - 1].Y;
                    angle = Math.Atan2(dy, dx) / Math.PI * 180;
                    return true;
                }

                length += dl;
            }

            position = pts[0];
            angle = 0;
            return false;
        }

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 void Update(float dt)
        {
            Vector3 aAxisY, aAxisZ;
            Vector3 bAxisY;
            Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
            Matrix3x3.Transform(ref aLocalAxisY, ref connectionA.orientationMatrix, out aAxisY);
            Matrix3x3.Transform(ref aLocalAxisZ, ref connectionA.orientationMatrix, out aAxisZ);
            Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
            Matrix3x3.Transform(ref bLocalAxisY, ref connectionB.orientationMatrix, out bAxisY);

            Quaternion rotation;
            Quaternion.GetQuaternionBetweenNormalizedVectors(ref worldAxisB, ref worldAxisA, out rotation);

            //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
            Vector3 twistMeasureAxis;
            Quaternion.Transform(ref bAxisY, 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 aAxisZ, out y);
            Vector3.Dot(ref twistMeasureAxis, ref aAxisY, out x);
            error = (float) Math.Atan2(y, x);

            //Debug.WriteLine("Angle: " + angle);

            //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 worldAxisA, ref worldAxisB, 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;

            //****** VELOCITY BIAS ******//
            //Compute the correction velocity.
            float errorReduction;
            springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
            biasVelocity = MathHelper.Clamp(-error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);

            //****** 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 ReadOnlyCollection<SimulatedMouseInput> GetSimulatedInput()
        {
            var magnitudes = new List<SimulatedMouseInput>();

            foreach (var slowMoveX in SlowMovements)
            {
                var slowMoveY = 0.0;
                var ceilX = (int)Math.Round(slowMoveX*50);
                var ceilY = (int)Math.Round(slowMoveY*50);
                var ceilMagnitude = Magnitude(ceilX, ceilY);
                var timeFactor = ceilMagnitude / Magnitude(slowMoveX, slowMoveY);

                SimulatedMouseInput mouseInputData;
                mouseInputData.x = ceilX;
                mouseInputData.y = ceilY;
                mouseInputData.time = MeasurementTime*timeFactor;
                mouseInputData.velocity = DecimalCheck(Velocity(ceilX, ceilY, mouseInputData.time));
                mouseInputData.angle = Math.Atan2(ceilY, ceilX);
                magnitudes.Add(mouseInputData);
            }

            for (double i = 5; i < MaxVelocity; i+=Increment)
            {
                SimulatedMouseInput mouseInputData;
                var ceil = (int)Math.Ceiling(i);
                var timeFactor = ceil / i;
                mouseInputData.x = ceil;
                mouseInputData.y = 0;
                mouseInputData.time = MeasurementTime * timeFactor;
                mouseInputData.velocity = DecimalCheck(Velocity(ceil, 0, mouseInputData.time));
                mouseInputData.angle = Math.Atan2(ceil, 0);
                magnitudes.Add(mouseInputData);
            }

            magnitudes.Sort((m1, m2) => m1.velocity.CompareTo(m2.velocity));

            return magnitudes.AsReadOnly();
        }

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 Vector3 CalculateProjectileVelocity(Vector3 localOrigin, WorldObject target, float projectileSpeed, out Vector3 origin, out Quaternion rotation)
        {
            var sourceLoc = PhysicsObj.Position.ACEPosition();
            var targetLoc = target.PhysicsObj.Position.ACEPosition();

            var crossLandblock = sourceLoc.Landblock != targetLoc.Landblock;

            var startPos = crossLandblock ? sourceLoc.ToGlobal(false) : sourceLoc.Pos;
            var endPos = crossLandblock ? targetLoc.ToGlobal(false) : targetLoc.Pos;

            var dir = Vector3.Normalize(endPos - startPos);

            var angle = Math.Atan2(-dir.X, dir.Y);

            rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)angle);

            origin = sourceLoc.Pos + Vector3.Transform(localOrigin, rotation);

            startPos += Vector3.Transform(localOrigin, rotation);
            endPos.Z += target.Height / GetAimHeight(target);

            var velocity = GetProjectileVelocity(target, startPos, dir, endPos, projectileSpeed, out float time);

            return velocity;
        }

private static double Angle(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb)
        {
            /* Complex plane
            * ab = cosA +i*sinA
            * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx)
            * atan2(y,x) computes the principal value of the argument function
            * applied to the complex number x+iy
            * Where x = ax*bx + ay*by
            * y = ax*by - ay*bx
            */
            double px = origin.X;
            double py = origin.Y;
            double ax = pa.X - px;
            double ay = pa.Y - py;
            double bx = pb.X - px;
            double by = pb.Y - py;
            double x = ax * by - ay * bx;
            double y = ax * bx + ay * by;
            double angle = Math.Atan2(x, y);
            return angle;
        }

internal static double ToAngleInRadians(Coordinates center, Coordinates waypoint)
        {
            var delta = waypoint - center;
            var rawRads =  Math.Atan2(delta.Y, delta.X);
            return rawRads >= 0? rawRads : Toolbox.TWO_PI + rawRads;
        }

public string GetPosition()
        {
            // 255 landblocks across * 192 meters for each landblock = 48,960 meters across Dereth
            if (Position.X < 0.0f || Position.Y < 0.0f || Position.X > 48960.0f || Position.Y > 48960.0f)
                return null;
            
            var lbx = (int)(Position.X / 192.0f);
            var lby = (int)(Position.Y / 192.0f);

            var x = Position.X % 192.0f;
            var y = Position.Y % 192.0f;

            var cellX = (int)(x / 24.0f);
            var cellY = (int)(y / 24.0f);

            var cell = cellX * 8 + cellY + 1;
            
            var objCellId = (uint)(lbx << 24 | lby << 16 | cell);

            var yaw = Math.Atan2(-Dir.X, Dir.Y);

            var q = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)yaw);

            // test if we are in any loaded indoor cells
            foreach (var landblock in LScape.Landblocks.Values)
            {
                // find origin in terms of this landblock
                var blockX = landblock.ID >> 24;
                var blockY = landblock.ID >> 16 & 0xFF;

                var blockX_start = blockX * 192.0f;
                var blockY_start = blockY * 192.0f;

                var blockPosX = Position.X - blockX_start;
                var blockPosY = Position.Y - blockY_start;

                var origin = new System.Numerics.Vector3(blockPosX, blockPosY, Position.Z);

                var envCells = landblock.get_envcells();

                foreach (var envCell in envCells)
                {
                    if (envCell.point_in_cell(origin))
                        return $"0x{envCell.ID:X8} [{blockPosX} {blockPosY} {Position.Z}] {q.W} {q.X} {q.Y} {q.Z}";
                }
            }

            // return outdoor location
            return $"0x{objCellId:X8} [{x} {y} {Position.Z}] {q.W} {q.X} {q.Y} {q.Z}";
        }

private static double BasinAngle(AdvancingFrontNode node)
        {
            double ax = node.Point.X - node.Next.Next.Point.X;
            double ay = node.Point.Y - node.Next.Next.Point.Y;
            return Math.Atan2(ay, ax);
        }

public Position InFrontOf(double distanceInFront, bool rotate180 = false)
        {
            float qw = RotationW; // north
            float qz = RotationZ; // south

            double x = 2 * qw * qz;
            double y = 1 - 2 * qz * qz;

            var heading = Math.Atan2(x, y);
            var dx = -1 * Convert.ToSingle(Math.Sin(heading) * distanceInFront);
            var dy = Convert.ToSingle(Math.Cos(heading) * distanceInFront);

            // move the Z slightly up and let gravity pull it down.  just makes things easier.
            var bumpHeight = 0.05f;
            if (rotate180)
            {
                var rotate = new Quaternion(0, 0, qz, qw) * Quaternion.CreateFromYawPitchRoll(0, 0, (float)Math.PI);
                return new Position(LandblockId.Raw, PositionX + dx, PositionY + dy, PositionZ + bumpHeight, 0f, 0f, rotate.Z, rotate.W);
            }
            else
                return new Position(LandblockId.Raw, PositionX + dx, PositionY + dy, PositionZ + bumpHeight, 0f, 0f, qz, qw);
        }

private bool AngleSign()
        {
            Point a = (_head.Next - _head);
            Point b = (_tail - _head);
            return Math.Atan2(a.Cross(b), a.Dot(b)) >= 0;
        }

public void set_vector_heading(Vector3 heading)
        {
            var normal = new Vector3(heading.X, heading.Y, heading.Z);
            if (Vec.NormalizeCheckSmall(ref normal)) return;

            var zDeg = 450.0f - ((float)Math.Atan2(normal.Y, normal.X)).ToDegrees();
            var zRot = -(zDeg % 360.0f).ToRadians();

            var xRot = (float)Math.Asin(normal.Z);

            var rotate = Quaternion.CreateFromYawPitchRoll(xRot, 0, zRot);
            set_rotate(rotate);
        }

public void DrawArrow(Vector2 start, Vector2 end, float length, float width, bool drawStartIndicator, Color color)
        {
            // Draw connection segment between start- and end-point
            DrawSegment(start, end, color);

            // Precalculate halfwidth
            float halfWidth = width / 2;

            // Create directional reference
            Vector2 rotation = (start - end);
            rotation.Normalize();

            // Calculate angle of directional vector
            float angle = (float)Math.Atan2(rotation.X, -rotation.Y);
            // Create matrix for rotation
            Matrix rotMatrix = Matrix.CreateRotationZ(angle);
            // Create translation matrix for end-point
            Matrix endMatrix = Matrix.CreateTranslation(end.X, end.Y, 0);

            // Setup arrow end shape
            Vector2[] verts = new Vector2[3];
            verts[0] = new Vector2(0, 0);
            verts[1] = new Vector2(-halfWidth, -length);
            verts[2] = new Vector2(halfWidth, -length);

            // Rotate end shape
            Vector2.Transform(verts, ref rotMatrix, verts);
            // Translate end shape
            Vector2.Transform(verts, ref endMatrix, verts);

            // Draw arrow end shape
            DrawSolidPolygon(verts, 3, color, false);

            if (drawStartIndicator)
            {
                // Create translation matrix for start
                Matrix startMatrix = Matrix.CreateTranslation(start.X, start.Y, 0);
                // Setup arrow start shape
                Vector2[] baseVerts = new Vector2[4];
                baseVerts[0] = new Vector2(-halfWidth, length / 4);
                baseVerts[1] = new Vector2(halfWidth, length / 4);
                baseVerts[2] = new Vector2(halfWidth, 0);
                baseVerts[3] = new Vector2(-halfWidth, 0);

                // Rotate start shape
                Vector2.Transform(baseVerts, ref rotMatrix, baseVerts);
                // Translate start shape
                Vector2.Transform(baseVerts, ref startMatrix, baseVerts);
                // Draw start shape
                DrawSolidPolygon(baseVerts, 4, color, false);
            }
        }

public static double[] bd2gcj(double lat, double lon)
        {
            double x = lon - 0.0065, y = lat - 0.006;
            double z = Math.Sqrt(x * x + y * y) - 0.00002 * Math.Sin(y * x_pi);
            double theta = Math.Atan2(y, x) - 0.000003 * Math.Cos(x * x_pi);
            double gg_lon = z * Math.Cos(theta);
            double gg_lat = z * Math.Sin(theta);
            return new double[] { gg_lat, gg_lon };
        }

public float GetAngle()
        {
            return (float)Math.Atan2(s, c);
        }

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 double Bearing(double[] a, double[] b)
		{
			var dx = (b[0] - a[0]) * _kx;
			var dy = (b[1] - a[1]) * _ky;
			if (dx == 0 && dy == 0)
			{
				return 0;
			}
			var bearing = Math.Atan2(dx, dy) * 180 / Math.PI;
			if (bearing > 180)
			{
				bearing -= 360;
			}
			return bearing;
		}

public void set_vector_heading(Vector3 heading)
        {
            var normal = heading;
            if (Vec.NormalizeCheckSmall(ref normal)) return;

            var zDeg = 450.0f - ((float)Math.Atan2(normal.Y, normal.X)).ToDegrees();
            var zRot = -(zDeg % 360.0f).ToRadians();

            var xRot = (float)Math.Asin(normal.Z);

            var rotate = Quaternion.CreateFromYawPitchRoll(xRot, 0, zRot);
            set_rotate(rotate);
        }

public override double Calculate()
		{

			// calc mean heading taking into account that eg 355� and 5� should result in 0� and not 180�
			// refs:
			// https://en.wikipedia.org/wiki/Mean_of_circular_quanreplacedies
			// https://rosettacode.org/wiki/Averages/Mean_angle
			// https://rosettacode.org/wiki/Averages/Mean_angle#C.23
			double cos = _angles.Sum(a => Math.Cos(a * DEG2RAD)) / _angles.Count;
			double sin = _angles.Sum(a => Math.Sin(a * DEG2RAD)) / _angles.Count;

			// round as we don't need super high precision
			double finalAngle = Math.Round(Math.Atan2(sin, cos) * RAD2DEG, 2);
			debugLogAngle(finalAngle, finalAngle);
			// stay within [0..<360]
			finalAngle = finalAngle < 0 ? finalAngle + 360 : finalAngle >= 360 ? finalAngle - 360 : finalAngle;
			debugLogAngle(finalAngle, finalAngle);

			return finalAngle;
		}

private void GameLoop_UpdateTicked(object sender, StardewModdingAPI.Events.UpdateTickedEventArgs e)
        {
            if (waveStarted)
                Game1.timeOfDay = 100 * (Game1.timeOfDay / 100);
            if (!Context.IsWorldReady || !Game1.player.IsMainPlayer || !(Game1.player.currentLocation is Farm))
                return;
            Farm farm = Game1.getFarm();

            if (farm == null)
                return;



            IEnumerable<Vector2> monsterPosList = farm.characters.Where(n => n is Monster && (n as Monster).health > 0).Select(m => m.position.Value);

            if (!monsterPosList.Any())
                return;

            foreach(Vector2 pos in monsterPosList)
            {
                KeyValuePair<Vector2, TerrainFeature> kvp = farm.terrainFeatures.Pairs.FirstOrDefault(k => k.Value is HoeDirt && (k.Value as HoeDirt).crop != null && k.Key == pos / Game1.tileSize);
                if(!kvp.Equals(default(KeyValuePair<Vector2, TerrainFeature>)))
                {
                    (farm.terrainFeatures[kvp.Key] as HoeDirt).destroyCrop(kvp.Key, false, farm);
                }
                    
            }

            foreach (KeyValuePair<Vector2, Object> crow in farm.objects.Pairs.Where(s => s.Value.bigCraftable && s.Value.Name.Contains("arecrow")))
            {
                MurderCrow mc = murderCrows[crow.Value.Name];
                IEnumerable<Vector2> monsters = monsterPosList.Where(m => Vector2.Distance(m, crow.Key * Game1.tileSize) < mc.range * Game1.tileSize).OrderBy(m => Vector2.Distance(m, crow.Key));

                if (monsters.Any() && ticksSinceMorning % (1000 / mc.rate) == 0)
                {
                    Vector2 dir = (monsters.Last() - crow.Key * Game1.tileSize);
                    dir.Normalize();

                    if(mc.name == "Rarecrow" || mc.name == "Iridium Scarecrow")
                    {
                        if (ticksSinceMorning % 1000  == 0)
                            farm.playSound("furnace"); 


                        float fire_angle = (float)Math.Atan2(dir.Y, dir.X);
                        if(mc.name == "Iridium Scarecrow")
                            fire_angle += (float)Math.Sin((double)((float)ticksSinceMorning % 180f) * 3.1415926535897931 / 180.0) * 25f;
                        else
                            fire_angle += (float)Math.Sin((double)((float)ticksSinceMorning % 10f) * 3.1415926535897931 / 180.0);

                        Vector2 shot_velocity = new Vector2((float)Math.Cos((double)fire_angle), (float)Math.Sin((double)fire_angle));
                        shot_velocity *= 10f;
                        BasicProjectile projectile = new BasicProjectile(mc.damage, 10, 0, 1, 0.196349546f, shot_velocity.X, shot_velocity.Y, crow.Key * Game1.tileSize, "", "", false, true, farm, Game1.MasterPlayer, false, null);
                        projectile.ignoreTravelGracePeriod.Value = true;
                        projectile.maxTravelDistance.Value = mc.range * 64;
                        farm.projectiles.Add(projectile);
                    }
                    else 
                        farm.projectiles.Add(new BasicProjectile(mc.damage, mc.ammoIndex, 0, 0, 0.3f, dir.X * shotVelocity, dir.Y * shotVelocity, crow.Key * Game1.tileSize, mc.hitSound, mc.fireSound, mc.explode, true, farm, Game1.MasterPlayer, mc.useTileSheet));
                }
            }
            ticksSinceMorning++;
        }

public override double Calculate()
		{
			// reverse order, latest in _angles is at [0]
			double[] angles = _angles.Reverse().ToArray();

			// since we cannot work directly on the angles (eg think about 355 and 5)
			// we convert to cartesian coordinates and apply filtering there
			// aproximation should be good enough for the use case of compreplaced filtering
			// differences occur only at the 2nd or 3rd digit after the decimal point

			double lastSin = Math.Sin(angles[0] * DEG2RAD);
			double lastCos = Math.Cos(angles[0] * DEG2RAD);

			debugLogAngle(angles[0], Math.Atan2(lastSin, lastCos) * RAD2DEG);

			for (int i = 1; i < angles.Length; i++)
			{
				double angle = angles[i];
				lastSin = _smoothingFactor * Math.Sin(angle * DEG2RAD) + (1 - _smoothingFactor) * lastSin;
				lastCos = _smoothingFactor * Math.Cos(angle * DEG2RAD) + (1 - _smoothingFactor) * lastCos;
				debugLogAngle(angles[i], Math.Atan2(lastSin, lastCos) * RAD2DEG);
			}

			// round, don't need crazy precision
			double finalAngle = Math.Round(Math.Atan2(lastSin, lastCos) * RAD2DEG, 2);
			debugLogAngle(finalAngle, finalAngle);
			// stay within [0..<360]
			finalAngle = finalAngle < 0 ? finalAngle + 360 : finalAngle >= 360 ? finalAngle - 360 : finalAngle;
			debugLogAngle(finalAngle, finalAngle);

			return finalAngle;
		}

public static double Atan2(double y, double x) {
            return Math.Atan2(y, x);
        }

protected float GetAngle(Point point1, Point point2)
		{
			if (point1.X == point2.X)
			{
				return (point1.Y < point2.Y) ? 0 : 180;
			}
			else if (point1.Y == point2.Y)
			{
				return (point1.X < point2.X) ? 270 : 90;
			}
			else
			{
				return (float) (
					Math.Atan2(point2.Y - point1.Y, point2.X - point1.X) * (180 / Math.PI)) - 90;
			}
		}

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();
        }

public static float get_heading(this Vector3 v)
        {
            var normal = new Vector3(v.X, v.Y, 0);
            if (Vec.NormalizeCheckSmall(ref normal))
                return 0.0f;

            var heading = (450.0f - ((float)Math.Atan2(normal.Y, normal.X)).ToDegrees()) % 360.0f;
            return heading;
        }

public static void buildlinearleastsquares(ref double[] x, ref double[] y, int n, ref double a, ref double b)
  {
    double num1 = (double) n;
    double num2 = 0.0;
    double num3 = 0.0;
    double num4 = 0.0;
    double num5 = 0.0;
    for (int index = 0; index <= n - 1; ++index)
    {
      num3 += x[index];
      num2 += AP.Math.Sqr(x[index]);
      num4 += y[index];
      num5 += x[index] * y[index];
    }
    double num6 = System.Math.Atan2(2.0 * num3, num2 - num1) / 2.0;
    double X1 = System.Math.Cos(num6);
    double X2 = System.Math.Sin(num6);
    double num7 = AP.Math.Sqr(X1) * num1 + AP.Math.Sqr(X2) * num2 - 2.0 * X2 * X1 * num3;
    double num8 = AP.Math.Sqr(X2) * num1 + AP.Math.Sqr(X1) * num2 + 2.0 * X2 * X1 * num3;
    double num9 = System.Math.Abs(num7) <= System.Math.Abs(num8) ? System.Math.Abs(num8) : System.Math.Abs(num7);
    double num10 = X1 * num4 - X2 * num5;
    double num11 = X2 * num4 + X1 * num5;
    double num12 = System.Math.Abs(num7) <= num9 * 5E-16 * 1000.0 ? 0.0 : num10 / num7;
    double num13 = System.Math.Abs(num8) <= num9 * 5E-16 * 1000.0 ? 0.0 : num11 / num8;
    a = X1 * num12 + X2 * num13;
    b = -(X2 * num12) + X1 * num13;
  }

public static double[] gcj2bd(double lat, double lon)
        {
            double x = lon, y = lat;
            double z = Math.Sqrt(x * x + y * y) + 0.00002 * Math.Sin(y * x_pi);
            double theta = Math.Atan2(y, x) + 0.000003 * Math.Cos(x * x_pi);
            double bd_lon = z * Math.Cos(theta) + 0.0065;
            double bd_lat = z * Math.Sin(theta) + 0.006;
            return new double[] { bd_lat, bd_lon };
        }

public void Rotate(Vector3 dir)
        {
            Rotation = Quaternion.CreateFromYawPitchRoll(0, 0, (float)Math.Atan2(-dir.X, dir.Y));
        }

public static float GetAngle(Vector3 dir)
        {
            var rads = Math.Atan2(dir.Y, dir.X);
            if (double.IsNaN(rads)) return 0.0f;

            var angle = rads * 57.2958f;
            return (float)angle;
        }

public static float DirectionToAngle(Vector2 direction)
   {
      return (float)Math.Atan2(-direction.X, direction.Y);
   }