public static unsafe void Main () {
        for (int i = 0; i < BufferSize; i++)
            Vectors[i] = new Vector3d(i * 0.5, (double)i, i * 1.5);

        Console.WriteLine("Add: {0:00000.00}ms", Time(TestAdd));
        Console.WriteLine("Add Overloaded: {0:00000.00}ms", Time(TestAddOverloaded));
    }

        // Perturb an orbit by a deltaV vector
        public static void Perturb(this Orbit orbit, Vector3d deltaVV, double UniversalTime, double deltaTime)
        {
            // If there is a deltaV, perturb orbit
            if (deltaVV.magnitude <= 0) return;

            // Transpose deltaVV Y and Z to match orbit frame
            Vector3d deltaVV_orbit = deltaVV.xzy;
            Vector3d position = orbit.getRelativePositionAtUT(UniversalTime);
            //Orbit orbit2 = orbit.Clone();
            //orbit2.UpdateFromStateVectors(position, orbit.getOrbitalVelocityAtUT(UniversalTime) + deltaVV_orbit, orbit.referenceBody, UniversalTime);
            //if (!double.IsNaN(orbit2.inclination) && !double.IsNaN(orbit2.eccentricity) && !double.IsNaN(orbit2.semiMajorAxis) && orbit2.timeToAp > deltaTime)
            //{
            //    orbit.inclination = orbit2.inclination;
            //    orbit.eccentricity = orbit2.eccentricity;
            //    orbit.semiMajorAxis = orbit2.semiMajorAxis;
            //    orbit.LAN = orbit2.LAN;
            //    orbit.argumentOfPeriapsis = orbit2.argumentOfPeriapsis;
            //    orbit.meanAnomalyAtEpoch = orbit2.meanAnomalyAtEpoch;
            //    orbit.epoch = orbit2.epoch;
            //    orbit.referenceBody = orbit2.referenceBody;
            //    orbit.Init();
            //    orbit.UpdateFromUT(UniversalTime);
            //}
            //else
            //{
                orbit.UpdateFromStateVectors(position, orbit.getOrbitalVelocityAtUT(UniversalTime) + deltaVV_orbit, orbit.referenceBody, UniversalTime);
                orbit.Init();
                orbit.UpdateFromUT(UniversalTime);
            //}
        }

 public HUDQuad(RenderSet render_set, Vector3d p, Vector3d s)
     : this(render_set, p,
          p + new Vector3d(s.X, 0.0, 0.0),
          p + new Vector3d(s.X, s.Y, 0.0),
          p + new Vector3d(0.0, s.Y, 0.0))
 {
 }

    public void RunCalculations(
        Vessel vessel,
        Quaternion gymbal)
    {
        // Calculations thanks to Mechjeb
        Vector3d CoM = vessel.findWorldCenterOfMass();
        Vector3d up = (CoM - vessel.mainBody.position).normalized;
        Vector3d velocityVesselOrbit = vessel.orbit.GetVel();
        Vector3d velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
        Vector3d radialPlus = Vector3d.Exclude(velocityVesselOrbit, up).normalized;
        Vector3d velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
        Vector3d velocityVesselSurfaceUnit = velocityVesselSurface.normalized;

        RadialPlus = gymbal * radialPlus;
        NormalPlus = gymbal * -Vector3d.Cross(radialPlus, velocityVesselOrbitUnit);
        ProgradeOrbit = gymbal * velocityVesselOrbitUnit;
        ProgradeSurface = gymbal * velocityVesselSurfaceUnit;

        if (vessel.patchedConicSolver.maneuverNodes.Count > 0)
        {
            Vector3d burnVector = vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(vessel.orbit);
            ManeuverPlus = gymbal * burnVector.normalized;
            ManeuverPresent = true;
        }
        else
        {
            ManeuverPresent = false;
        }
    }

 public static Ray Reflect(Ray incoming, Vector3d normal)
 {
     Ray reflected = new Ray(
         incoming.Origin,
         Reflect(incoming.Direction, normal));
     return reflected;
 }

 public static Vector3d GetRotateVector(Vector3d v, double xr, double yr, double zr)
 {
     v = v.TransformBy(Matrix3d.Rotation(xr * System.Math.PI / 180, Vector3d.XAxis, Point3d.Origin));
     v = v.TransformBy(Matrix3d.Rotation(yr * System.Math.PI / 180, Vector3d.YAxis, Point3d.Origin));
     v = v.TransformBy(Matrix3d.Rotation(zr * System.Math.PI / 180, Vector3d.ZAxis, Point3d.Origin));
     return v;
 }

            public override Mesh Generate(Point3d P, Vector3d V)
            {
                Vector3d diffx;
                Vector3d diffy;
                Vector3d diffz = new Vector3d(V);
                double proj;
                diffx = new Vector3d(0, 0, 1);
                proj = System.Math.Abs(Vector3d.Multiply(diffz, diffx));

                if (0.99 < proj && 1.01 > proj) diffx = new Vector3d(1, 0, 0);
                diffy = diffz;
                diffy = Vector3d.CrossProduct(diffy, diffx);
                diffx = Vector3d.CrossProduct(diffy, diffz);
                diffx.Unitize();
                diffy.Unitize();
                diffz.Unitize();

                Mesh M = new Mesh();
                M.Vertices.SetVertex(0, P + diffz * 0.25);
                M.Vertices.SetVertex(1, P + (diffx * 0.7072 + diffy * -.5 + diffz * -.5) * 0.25);
                M.Vertices.SetVertex(2, P + (diffy + diffz * -.5) * 0.25);
                M.Vertices.SetVertex(3, P + (diffx * -0.7072 + diffy * -.5 + diffz * -.5) * 0.25);

                M.Faces.SetFace(0, 0, 1, 2);
                M.Faces.SetFace(1, 0, 2, 3);
                M.Faces.SetFace(2, 0, 3, 1);
                M.Faces.SetFace(3, 3, 2, 1);
                M.FaceNormals.ComputeFaceNormals();
                //M.ComputeVertexNormals();

                return M;
            }

		/// <summary>
		/// Ctor
		/// </summary>
		/// <param name="pos"></param>
		/// <param name="normal"></param>
		/// <param name="color"></param>
		public Point3D(Vector3d pos, Vector3d normal, Color color)
			: this()
		{
			Position = pos;
			Normal = normal;
			Color = color;
		}

 private static bool IsHidden(Vector3d point)
 {
     Vector3d camera = ScaledSpace.ScaledToLocalSpace(
     PlanetariumCamera.Camera.transform.position);
     foreach (CelestialBody body in hiding_bodies_) {
       Vector3d camera_to_point = point - camera;
       Vector3d camera_to_body = body.position - camera;
       double inner_product = Vector3d.Dot(camera_to_point, camera_to_body);
       double r_squared = body.Radius * body.Radius;
       // The projections on the camera-body axis of |point| and of the horizon
       // have lengths |inner_product| / d and d - r^2/d, where d is the distance
       // between the camera and the body and r is the body's radius, thus if
       // |inner_product| < d^2 - r^2, |point| is above the plane passing
       // through the horizon.
       // Otherwise, we check whether |point| is within the cone hidden from the
       // camera, by comparing the squared cosines multiplied by
       // d^2|camera_to_point|^2.
       // In addition, we check whether we're inside the body (this covers the
       // cap above the horizon plane and below the surface of the body, which
       // would otherwise be displayed).
       double d_squared_minus_r_squared =
       camera_to_body.sqrMagnitude - r_squared;
       if ((body.position - point).sqrMagnitude < r_squared ||
       (inner_product > d_squared_minus_r_squared &&
        inner_product * inner_product >
        camera_to_point.sqrMagnitude * d_squared_minus_r_squared)) {
     return true;
       }
     }
     return false;
 }

 /// <summary>
 /// Initializes a new instance of the WorldBoxEnvironmentComponent class.
 /// </summary>
 public PolysurfaceEnvironmentComponent()
   : base("Polysurface Environment", "PolysrfEnv",
       "A 3D Polysurface Environment.", RS.icon_polysurfaceEnvironment, "646e7585-d3b0-4ebd-8ce3-8efc5cd6b7d9")
 {
   brep = new Brep();
   borderDir = Vector3d.Zero;
 }

        /// <summary>
        /// Проводит десериалзиацию
        /// </summary>
        /// <param name="package">Содержимое пакета</param>
        /// <returns>Структура из трех величин</returns>
        public override Vector3d Do(string package)
        {
            string sub = "";
            Vector3d result = new Vector3d();

            int index1 = 0;
            int index2 = package.LastIndexOf('>');

            for (int i = index2 - 1; i >= 0; i--)
            {
                if (package[i] == '<')
                {
                    index1 = i;
                    break;
                }
            }

            if ((index1 != -1) && (index2 != -1) && (index2 >= index1))
                sub = package.Substring(index1 + 1, index2 - index1 - 1);
            else
                throw new ExceptionServer("Block borders are not found");

            string[] parameters = sub.Split(';');

            result.X = double.Parse(parameters[0].Replace(',', '.'), CultureInfo.InvariantCulture); // Pitch
            result.Y = double.Parse(parameters[1].Replace(',', '.'), CultureInfo.InvariantCulture); // Roll
            result.Z = double.Parse(parameters[2].Replace(',', '.'), CultureInfo.InvariantCulture); // Yaw

            return result;
        }

 public static Vector3d Cross(Vector3d lhs, Vector3d rhs)
 {
     return new Vector3d(
         lhs.y * rhs.z - lhs.z * rhs.y,
         lhs.z * rhs.x - lhs.x * rhs.z,
         lhs.x * rhs.y - lhs.y * rhs.x);
 }

        public Vector3d Compute(Vector3d error, Vector3d omega, Vector3d Wlimit)
        {
            derivativeAct = Vector3d.Scale(omega, Kd);
            Wlimit = Vector3d.Scale(Wlimit, Kd);

            // integral actíon + Anti Windup
            intAccum.x = (Math.Abs(derivativeAct.x) < 0.6 * max) ? intAccum.x + (error.x * Ki.x * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.x;
            intAccum.y = (Math.Abs(derivativeAct.y) < 0.6 * max) ? intAccum.y + (error.y * Ki.y * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.y;
            intAccum.z = (Math.Abs(derivativeAct.z) < 0.6 * max) ? intAccum.z + (error.z * Ki.z * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.z;

            propAct = Vector3d.Scale(error, Kp);

            Vector3d action = propAct + intAccum;

            // Clamp (propAct + intAccum) to limit the angular velocity:
            action = new Vector3d(Math.Max(-Wlimit.x, Math.Min(Wlimit.x, action.x)),
                                  Math.Max(-Wlimit.y, Math.Min(Wlimit.y, action.y)),
                                  Math.Max(-Wlimit.z, Math.Min(Wlimit.z, action.z)));

            // add. derivative action
            action += derivativeAct;

            // action clamp
            action = new Vector3d(Math.Max(min, Math.Min(max, action.x)),
                                  Math.Max(min, Math.Min(max, action.y)),
                                  Math.Max(min, Math.Min(max, action.z)));
            return action;
        }

 public static double AngleBetweenTwoVectors(Vector3d a, Vector3d b)
 {
     double temp = (a.X * b.X + a.Y * b.Y + a.Z * b.Z) / (a.Length * b.Length);
     if (temp > 1.0) temp = 1.0;
     else if (temp < -1.0) temp = -1.0;
     return Math.Acos(temp);
 }

        /// <summary>
        /// Calculates distance between a hole edge and all other holes. This distance is used to calculate the force between the points and assign an acceleration to a point.
        /// </summary>
        /// <param name="others"></param>
        /// <param name="p"></param>
        /// <returns></returns>
        private void CalculateAcceleration(IList<Hole> others)
        {
            this.newAccelerationVector = Vector3d.Zero;
            double minimumDenominatorToAvoidDivideByZero = 5;
            for (int i = 0; i < others.Count; i++)
            {
                Hole other = others[i];
                Vector3d VectorPointToPointInfluence = other.point - this.point;
                double distanceBetweenPoints = VectorPointToPointInfluence.Length;
                if (distanceBetweenPoints < double.Epsilon)
                {
                    continue;
                }

                double distanceBetweenHoles = distanceBetweenPoints - other.radius - this.radius;
                if (distanceBetweenHoles <= 0) //checks if holes intersect.
                {
                    distanceBetweenHoles = 0;
                }

                VectorPointToPointInfluence.Unitize();
                VectorPointToPointInfluence *= -this.force / (minimumDenominatorToAvoidDivideByZero + distanceBetweenHoles * distanceBetweenHoles);
                VectorPointToPointInfluence = AdjustAccelerationForColourSensitivity(VectorPointToPointInfluence, other);
                this.newAccelerationVector += VectorPointToPointInfluence;
            }

            this.newAccelerationVector = this.ConstrainVectorToMeshFace(this.newAccelerationVector);
        }

        /// <summary>
        /// Creates a new Polyline that is the result of projecting the Polyline3d parallel to 'direction' onto 'plane' and returns it.
        /// </summary>
        /// <param name="pline">The polyline to project.</param>
        /// <param name="plane">The plane onto which the curve is to be projected.</param>
        /// <param name="direction">Direction (in WCS coordinates) of the projection.</param>
        /// <returns>The projected Polyline.</returns>
        public static Polyline GetProjectedPolyline(this Polyline3d pline, Plane plane, Vector3d direction)
        {
            if (plane.Normal.IsPerpendicularTo(direction, new Tolerance(1e-9, 1e-9)))
                return null;

            return GeomExt.ProjectPolyline(pline, plane, direction);
        }

    protected override Vector3d CalculateDesiredVelocity()
    {
      Vector3d desired = new Vector3d();
      //Predict the vehicle's future location
      Vector3d predict = agent.Velocity3D;
      predict.Unitize();
      predict = predict * predictionDistance;
      predictLoc = agent.Position3D + predict;

      //Find the normal point along the path
      double t;
      path.ClosestPoint(predictLoc, out t);
      pathPt = path.PointAt(t);

      //Move a little further along the path and set a target


      //If we are off the path, seek that target in order to stay on the path
      double distance = pathPt.DistanceTo(predictLoc);
      if (distance > radius)
      {
        pathPt = path.PointAt(t + pathTargetDistance);
        pathPt = agent.Environment.MapTo2D(pathPt);
        // Seek that point
        desired = Util.Agent.Seek(agent, pathPt);
      }
      return desired;
    }

        //Methods

        List<Vector3d> calcNodalWindLoads(List<Vector3d> vertexNormals, Vector3d wind, bool scale)
        {
            List<Vector3d> windload = new List<Vector3d>();

            double w = wind.Length;                                 //kN/m2
            Vector3d wDir = new Vector3d(wind);
            wDir.Unitize();

            for(int i=0; i<vertexNormals.Count; i++)
            {
                //Calculate projection onto wind direction
                Vector3d vN = vertexNormals[i];
                double vertexArea = vN.Length;                      //m2
                vN.Unitize();

                double dotProduct = Vector3d.Multiply(vN, wDir);

                //Calculate wind force
                Vector3d force = vN * vertexArea * w * 1e3;         //N

                if(dotProduct < 0.0)
                {
                    force.Reverse();
                }

                if (scale)
                {
                    force *= Math.Abs(dotProduct);
                }

                windload.Add(force);
            }

            return windload;
        }

 /// <summary>
 /// Base initializer for all shapes
 /// </summary>
 /// <param name="vertices">The number of vertices that make up the shape</param>
 /// <param name="Position">The zero position of the vertices (these are shifted around with Calculate())</param>
 public Shape(int vertices, Vector3d Position)
 {
     this.Vertices = new Vector3d[vertices];
     for (int i = 0; i < Vertices.Length; i++) {
         Vertices[i] = Position;
     }
 }

    protected override Vector3d CalculateDesiredVelocity()
    {
      Vector3d desired = new Vector3d();
      int count = 0;

      foreach (IQuelea neighbor in neighbors)
      {
        //Point3d neighborPosition2D = agent.Environment.ClosestRefPoint(neighbor.Position3D);
        Point3d neighborPosition2D = agent.Environment.Wrap ? wrappedPositions[count] : neighbor.Position;
        //Adding up all the others' location
        desired = desired + (Vector3d)neighborPosition2D;
        //For an average, we need to keep track of how many boids
        //are in our vision.
        count++;
      }

      if (count > 0)
      {
        //We desire to go in that direction at maximum speed.
        desired = desired / count;
        desired = Util.Agent.Seek(agent, new Point3d(desired));
      }
      //Seek the average location of our neighbors.
      return desired;
    }