public static MeshGeometry3D AddPlaneToMesh(MeshGeometry3D mesh, Vector3D normal, Point3D upperLeft, Point3D lowerLeft, Point3D lowerRight, Point3D upperRight)
        {
            int offset = mesh.Positions.Count;

            mesh.Positions.Add(upperLeft);
            mesh.Positions.Add(lowerLeft);
            mesh.Positions.Add(lowerRight);
            mesh.Positions.Add(upperRight);

            mesh.Normals.Add(normal);
            mesh.Normals.Add(normal);
            mesh.Normals.Add(normal);
            mesh.Normals.Add(normal);

            mesh.TextureCoordinates.Add(new Point(0, 0));
            mesh.TextureCoordinates.Add(new Point(0, 1));
            mesh.TextureCoordinates.Add(new Point(1, 1));
            mesh.TextureCoordinates.Add(new Point(1, 0));

            mesh.TriangleIndices.Add(offset + 0);
            mesh.TriangleIndices.Add(offset + 1);
            mesh.TriangleIndices.Add(offset + 2);
            mesh.TriangleIndices.Add(offset + 0);
            mesh.TriangleIndices.Add(offset + 2);
            mesh.TriangleIndices.Add(offset + 3);

            return mesh;
        }

        /// <summary>
        /// Calculates the texture for the specified model.
        /// </summary>
        /// <param name="model">
        /// The model.
        /// </param>
        /// <param name="mesh">
        /// The mesh.
        /// </param>
        public override void Calculate(TerrainModel model, MeshGeometry3D mesh)
        {
            var normals = MeshGeometryHelper.CalculateNormals(mesh);
            var texcoords = new PointCollection();
            var up = new Vector3D(0, 0, 1);
            for (int i = 0; i < normals.Count; i++)
            {
                double slope = Math.Acos(Vector3D.DotProduct(normals[i], up)) * 180 / Math.PI;
                double u = slope / 40;
                if (u > 1)
                {
                    u = 1;
                }

                if (u < 0)
                {
                    u = 0;
                }

                texcoords.Add(new Point(u, u));
            }

            this.TextureCoordinates = texcoords;
            this.Material = MaterialHelper.CreateMaterial(this.Brush);
        }

 public Plant()
 {
     var x = new Vector3D(1, 0, 0);
     var r1 = new RotateTransform3D(new AxisAngleRotation3D(x, 80));
     var r2 = new RotateTransform3D(new AxisAngleRotation3D(x, -70));
     var r3 = new RotateTransform3D(new AxisAngleRotation3D(x, -10));
     var t1 = new TranslateTransform3D(0, 0, 0.5);
     var t2 = new TranslateTransform3D(0, 0, 0.7);
     var t3 = new TranslateTransform3D(0, 0, 1.0);
     var s1 = new ScaleTransform3D(0.5, 0.5, 0.5);
     var s2 = new ScaleTransform3D(0.3, 0.3, 0.3);
     var s3 = new ScaleTransform3D(0.8, 0.8, 0.8);
     var m1 = new Transform3DGroup();
     m1.Children.Add(r1);
     m1.Children.Add(s1);
     m1.Children.Add(t1);
     var m2 = new Transform3DGroup();
     m2.Children.Add(r2);
     m2.Children.Add(s2);
     m2.Children.Add(t2);
     var m3 = new Transform3DGroup();
     m3.Children.Add(r3);
     m3.Children.Add(s3);
     m3.Children.Add(t3);
     T1 = m1;
     T2 = m2;
     T3 = m3;
 }

        //trajectory() in MATLAB; calculation position of end effector and ideal orientation of the needle
        public dof4 end_effector(Vector3D forearm_orientation, double delta_theta)
        {
            needle_holder_twist = needle_holder_twist  + t_incr;
            update_needle_holder_position();
            dof4 needle_holder;
            needle_holder.pos = needle_holder_position;
            needle_holder.twist = needle_holder_twist;

            print_vector(needle_holder_position);
            print_double(needle_holder_twist);
            /*
            set_circle_center(); // calculating center of needle
            // calculating position
            DOF.pos = new Vector3D(xc + r * Math.Sin(t), yc + r * Math.Cos(t), zc);

            // calculation orientation
            Vector3D centric1 = new Vector3D(r * Math.Sin(t), r * Math.Cos(t), 0);
            Vector3D centric2 = new Vector3D(r * Math.Sin(t + t_incr), r * Math.Cos(t + t_incr), 0);
            Vector3D normal = new Vector3D();
            //Vector3D ideal_needle_orientation = new Vector3D();
            normal = Vector3D.CrossProduct(centric1, centric2); // normal of circle plane
            ideal_needle_orientation = Vector3D.CrossProduct(centric1, normal); // which is the tangent of the path
            Vector3D optimal_needle_orientation = new Vector3D();
            optimal_needle_orientation = minimizer(forearm_orientation);
            DOF.ori = optimal_needle_orientation;
            */
            return needle_holder;
        }

		public static Matrix3D EulerAngleToMatrix(Vector3D pEulersAngles)
		{
			NewtonMatrix aNewtonMatrix = new NewtonMatrix(Matrix3D.Identity);
			Newton.NewtonSetEulerAngle(new NewtonVector3(pEulersAngles).NWVector3,
				aNewtonMatrix.NWMatrix);
			return aNewtonMatrix.ToDirectX();
		}

 public WWLineSegment(Point3D startPos, Vector3D dirNormalized, double length, double intensity)
 {
     StartPos = startPos;
     Direction = dirNormalized;
     Length = length;
     Intensity = intensity;
 }

        public static double angle(Atom a, Atom b)
        {
            Vector3D va = new Vector3D(a.getX(), a.getY(), a.getZ());
                Vector3D vb = new Vector3D(b.getX(), b.getY(), b.getZ());

                return Vector3D.AngleBetween(va, vb);
        }

 public Vector3D NormalAt(Vector3D intersectionPoint)
 {
     //Assert actually an intersection?
     var surfaceNormal = intersectionPoint - CenterPoint;
     surfaceNormal.Normalize();
     return surfaceNormal;
 }

        public double? Test(Vector3D renderPoint, Vector3D direction)
        {
            var lineToCircle = renderPoint - CenterPoint;
            var B = 2 * Vector3D.DotProduct(direction, lineToCircle);
            var C = lineToCircle.LengthSquared - Math.Pow(Radius, 2);

            var determinate = Math.Pow(B, 2) - 4 * C;
            if (determinate >= 0)
            {
                var diff = Math.Sqrt(determinate);
                var tintersect1 = (-B - diff) / 2;
                var tintersect2 = (-B + diff) / 2;

                if (tintersect1 > tintersect2)
                {
                    var temp = tintersect2;
                    tintersect2 = tintersect1;
                    tintersect1 = temp;
                }
                if (tintersect1 < 0)
                {
                    tintersect1 = tintersect2;
                }
                if (tintersect1 >= 0)
                {
                    return tintersect1;
                }
            }
            return null;
        }

        public static bool ModelRayIntersection(WW3DModel model, Point3D rayOrig, Vector3D rayDir, out Point3D hitPos, out Vector3D hitSurfaceNormal, out double rayLength)
        {
            rayLength = double.MaxValue;
            hitPos = new Point3D();
            hitSurfaceNormal = new Vector3D();

            var points = model.TriangleList();
            var indices = model.IndexList();

            for (int i = 0; i < indices.Length/3; ++i) {
                Point3D pos;
                Vector3D surfaceNormal;
                double distance;
                if (!TriangleRayIntersect(points[indices[i * 3 + 0]], points[indices[i * 3 + 1]], points[indices[i * 3 + 2]], rayOrig, rayDir, out pos, out surfaceNormal, out distance)) {
                    continue;
                }
                if (distance < rayLength) {
                    hitPos = pos;
                    hitSurfaceNormal = surfaceNormal;
                    rayLength = distance;
                }
            }

            return rayLength != double.MaxValue;
        }

        public static bool TriangleRayIntersect(Point3D p0, Point3D p1, Point3D p2, Point3D rayOrig, Vector3D rayDir, out Point3D hitPos, out Vector3D surfaceNormal, out double distance)
        {
            var edge01 = p1 - p0;
            var edge02 = p2 - p0;

            surfaceNormal = Vector3D.CrossProduct(edge01, edge02);
            surfaceNormal.Normalize();
            hitPos = new Point3D();
            distance = double.MaxValue;

            var p = Vector3D.CrossProduct(rayDir, edge02);
            var det = Vector3D.DotProduct(edge01, p);
            if (det < float.Epsilon) {
                // レイとトライアングルが平行 or backface
                return false;
            }

            var tvec = rayOrig - p0;
            var u = Vector3D.DotProduct(tvec,p);
            if (u < 0 || det < u) {
                return false;
            }

            var qvec = Vector3D.CrossProduct(tvec, edge01);
            var v = Vector3D.DotProduct(rayDir, qvec);
            if (v < 0 || det < u+v) {
                return false;
            }

            distance = Vector3D.DotProduct(edge02, qvec) / det;
            hitPos = rayOrig + distance * rayDir;
            return true;
        }

        public double Ver(DrawingContext dc, Target TargetA, Target TargetB, bool show)
        {
            //3D
            Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, 0);// TargetA.point3D().Z - TargetB.point3D().Z);
            Vector3D vectorB = new Vector3D(0, 1, 0);

            //2D
            //Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
            //Vector3D vectorB = new Vector3D(0, TargetA.point2D().Y - TargetB.point2D().Y, 0);

            double theta = Math.Abs(Vector3D.AngleBetween(vectorA, vectorB));
            if (TargetA.point3D().X < TargetB.point3D().X) theta = -theta;

            if (show)       //show angle text
            {
                dc.DrawText(new FormattedText(theta.ToString("f0"),
                CultureInfo.GetCultureInfo("en-us"),
                FlowDirection.LeftToRight,
                new Typeface("Verdana"),
                25, brushLemonChiffon),
                new Point(TargetB.point2D().X - 35, TargetB.point2D().Y - 35));

                dc.DrawLine(PenLemonChiffon, TargetA.point2D(), TargetB.point2D());    //show angle line 
                //dc.DrawLine(PenLemonChiffon, new Point(TargetB.point2D().X, TargetA.point2D().Y), new Point(TargetB.point2D().X, TargetB.point2D().Y));  //show Vertical line
            }
            return theta;
        }

        public double Hor(DrawingContext dc, Target TargetA, Target TargetB ,bool show)
        {          
            //3D
            Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, TargetA.point3D().Z - TargetB.point3D().Z);
            Vector3D vectorB = new Vector3D(0, 1, 0);
            
            //2D
            //Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
            //Vector3D vectorB = new Vector3D(1, 0, 0);

            double theta = Math.Abs(Vector3D.AngleBetween(vectorA, vectorB));
            theta = 90 - theta;
            //if (TargetA.point3D().Y < TargetB.point3D().Y) theta = -theta;

            if (show)       //show angle text
            {
                dc.DrawText(new FormattedText(theta.ToString("f0"),
                CultureInfo.GetCultureInfo("en-us"),
                FlowDirection.LeftToRight,
                new Typeface("Verdana"),
                25, brushDeepSkyBlue),
                new Point(TargetB.point2D().X - 35, TargetB.point2D().Y - 35));

                dc.DrawLine(PenDeepSkyBlue, TargetA.point2D(), TargetB.point2D());    //show angle line 
                dc.DrawLine(PenDeepSkyBlue, new Point(TargetA.point2D().X, TargetB.point2D().Y), TargetB.point2D());
            }
            return theta;
        }

        /// <summary>
        /// Helper method creates a triangle fan to close the ends of the cylinder.
        /// </summary>
        void CreateCap(int tessellation, float height, float radius, Vector3D normal)
        {
            // Create cap indices.
            for (int i = 0; i < tessellation - 2; i++)
            {
                if (normal.Y > 0)
                {
                    AddIndex(CurrentVertex);
                    AddIndex(CurrentVertex + (i + 1) % tessellation);
                    AddIndex(CurrentVertex + (i + 2) % tessellation);
                }
                else
                {
                    AddIndex(CurrentVertex);
                    AddIndex(CurrentVertex + (i + 2) % tessellation);
                    AddIndex(CurrentVertex + (i + 1) % tessellation);
                }
            }

            // Create cap vertices.
            for (int i = 0; i < tessellation; i++)
            {
                Vector3D vertex = GetCircleVector(i, tessellation) * radius +
                                   normal * height;
                Point3D vertexPoint = new Point3D(vertex.X, vertex.Y, vertex.Z);
                AddVertex(vertexPoint, normal);
            }
        }

 public WWFirCoefficient(double delaySecond, Vector3D soundDir, double gain, bool isDirect)
 {
     DelaySecond = delaySecond;
     SoundDirection = soundDir;
     Gain = gain;
     IsDirect = isDirect;
 }

        public void CreateSphere(Vector3D pRadius, Matrix3D pOffsetMatrix)
        {
            NewtonMatrix aMatrix = new NewtonMatrix(pOffsetMatrix);
            m_Handle = Newton.NewtonCreateSphere(m_World.Handle, (float)pRadius.X, (float)pRadius.Y, (float)pRadius.Z, aMatrix.NWMatrix);

            CHashTables.Collision.Add(m_Handle, this);
        }

        public override void Load()
        {
            IsEnabled = true;

            try
            {
                _wiimote = new Wiimote();
                _wiimote.Connect();
                _wiimote.InitializeMotionPlus();
                _wiimote.WiimoteChanged += wiimote_WiimoteChanged;

                _wiimote.SetRumble(true);
                _wiimote.SetLEDs(true, false, false, true);
                Thread.Sleep(40);
                _wiimote.SetRumble(false);

                RawPosition = new Vector3D();
            }
            catch (Exception exc)
            {
                Logger.Instance.Error(exc.Message, exc);
                try
                {
                    _wiimote.SetLEDs(false, false, false, false);
                }
                catch (Exception exception)
                {
                    Logger.Instance.Error(exception.Message, exception);
                }
                IsEnabled = false;
            }
        }

 public static void SetToVector(this TranslateTransform3D translateTransform3D, Vector3D vector3D)
 {
     Contract.Requires(translateTransform3D != null);
     translateTransform3D.OffsetX = vector3D.X;
     translateTransform3D.OffsetY = vector3D.Y;
     translateTransform3D.OffsetZ = vector3D.Z;
 }

 public SpatialEntity(string name, string model, Vector3D position, Quaternion rotation)
 {
     Name = name;
     Model = model;
     Position = position;
     Rotation = rotation;
 }

        /// <summary>
        /// Create a look at matrix using U, V, and N vectors
        /// </summary>
        /// <param name="eye">Location of the eye</param>
        /// <param name="lookDirection">Direction to look</param>
        /// <param name="up">The up vector</param>
        /// <returns>Transform matrix from world space to camera space.</returns>
        public static Matrix3D LookAt(Vector3D eye, Vector3D lookDirection, Vector3D up)
        {
            var n = lookDirection;
            n.Normalize();

            var v = up - (Vector3D.DotProduct(up, n) * n);
            v.Normalize();

            // The "-" below makes this be a right-handed uvn space so we aren't
            // negating the x component in SensorToScreenCoordinatesTransform.
            // It also makes SensorToScreenPositionTransform give an immediately
            // usable transform without having to flip the X.
            var u = -Vector3D.CrossProduct(n, v);

            var lookAt = new Matrix3D(
                u.X,
                v.X,
                n.X,
                0,
                u.Y,
                v.Y,
                n.Y,
                0,
                u.Z,
                v.Z,
                n.Z,
                0,
                -Vector3D.DotProduct(u, eye),
                -Vector3D.DotProduct(v, eye),
                -Vector3D.DotProduct(n, eye),
                1);

            return lookAt;
        }