public Model3DGroup Create(Color modelColor,string pictureName, Point3D startPos, double maxHigh)
        {
            try
            {
                Uri inpuri = new Uri(@pictureName, UriKind.Relative);
                BitmapImage bi = new BitmapImage();
                bi.BeginInit();
                bi.UriSource = inpuri;
                bi.EndInit();
                ImageBrush imagebrush = new ImageBrush(bi);
                imagebrush.Opacity = 100;
                imagebrush.Freeze();

                Point[] ptexture0 = { new Point(0, 0), new Point(0, 1), new Point(1, 0) };
                Point[] ptexture1 = { new Point(1, 0), new Point(0, 1), new Point(1, 1) };

                SolidColorBrush modelbrush = new SolidColorBrush(modelColor);
                Model3DGroup cube = new Model3DGroup();
                Point3D uppercircle = startPos;
                modelbrush.Freeze();
                uppercircle.Y = startPos.Y + maxHigh;
                cube.Children.Add(CreateEllipse2D(modelbrush, uppercircle, _EllipseHigh, new Vector3D(0, 1, 0)));
                cube.Children.Add(CreateEllipse2D(modelbrush, startPos, _EllipseHigh, new Vector3D(0, -1, 0)));
                cube.Children.Add(CreateEllipse3D(imagebrush, startPos, _EllipseHigh, maxHigh, ptexture0));
                return cube;
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

 internal static void SetPoint(this User user, JointID joint, Point3D newPoint)
 {
     switch (joint)
     {
         case JointID.Head:
             user.Head = newPoint;
             break;
         case JointID.AnkleLeft:
             user.AnkleLeft = newPoint;
             break;
         case JointID.ElbowLeft:
             user.ElbowLeft = newPoint;
             break;
         case JointID.FootLeft:
             user.FootLeft = newPoint;
             break;
         case JointID.HandLeft:
             user.HandLeft = newPoint;
             break;
         case JointID.KneeLeft:
             user.KneeLeft = newPoint;
             break;
         case JointID.ShoulderLeft:
             user.ShoulderLeft = newPoint;
             break;
         case JointID.HipLeft:
             user.HipLeft = newPoint;
             break;
         case JointID.ShoulderCenter:
             user.ShoulderCenter = newPoint;
             break;
         case JointID.AnkleRight:
             user.AnkleRight = newPoint;
             break;
         case JointID.ElbowRight:
             user.ElbowRight = newPoint;
             break;
         case JointID.FootRight:
             user.FootRight = newPoint;
             break;
         case JointID.HandRight:
             user.HandRight = newPoint;
             break;
         case JointID.KneeRight:
             user.KneeRight = newPoint;
             break;
         case JointID.HipRight:
             user.HipRight = newPoint;
             break;
         case JointID.ShoulderRight:
             user.ShoulderRight = newPoint;
             break;
         case JointID.Spine:
             user.Spine = newPoint;
             break;
         case JointID.HipCenter:
             user.HipCenter = newPoint;
             break;
     }
 }

        protected override Geometry3D CreateMesh()
        {
            _radius = Radius;
            _position = Position;

            MeshGeometry3D mesh = new MeshGeometry3D();
            Point3D prevPoint = PointForAngle(0);
            Vector3D normal = new Vector3D(0, 0, 1);

            const int div = 180;
            for (int i = 1; i <= div; ++i)
            {
                double angle = 2 * Math.PI / div * i;
                Point3D newPoint = PointForAngle(angle);
                mesh.Positions.Add(prevPoint);
                mesh.Positions.Add(_position);
                mesh.Positions.Add(newPoint);
                mesh.Normals.Add(normal);
                mesh.Normals.Add(normal);
                mesh.Normals.Add(normal);
                prevPoint = newPoint;
            }

            mesh.Freeze();
            return mesh;
        }

        public List<StarSystem> BuildStarSystemCollection(Point3D midPoint, double range)
        {
            List<StarSystem> returnList = new List<StarSystem>();

            double minX = midPoint.X - range;
            double minY = midPoint.Y - range;
            double minZ = midPoint.Z - range;
            double maxX = midPoint.X + range;
            double maxY = midPoint.Y + range;
            double maxZ = midPoint.Z + range;

            foreach (var systemlist in milkyWay
                .Where(system => system.Location.X >= minX && system.Location.X <= maxX &&
                                 system.Location.Y >= minY && system.Location.Y <= maxY &&
                                 system.Location.Z >= minZ && system.Location.Z <= maxZ))
            {
                if (calculateDistance(midPoint, new Point3D(systemlist.Location.X, systemlist.Location.Y, systemlist.Location.Z)) <= range)
                {
                    returnList.Add(systemlist);
                }
            }

            centerPoint = midPoint;

            return returnList;
        }

        public Star()
        {
            starColor = foregroundBrush;

            starGFX = new Ellipse();
            starSEL = new Ellipse();

            starCanvas = new Canvas();
            starLabel = new Label();

            starSEL.Visibility = Visibility.Hidden;
            starGFX.Fill = foregroundBrush;

            starLabel.FontFamily = new FontFamily(new Uri("pack://application:,,,/Fonts/"), "./#Euro Caps");
            starLabel.Foreground = fontBrush;

            starCanvas.Children.Add(starLabel);
            starCanvas.Children.Add(starGFX);
            starCanvas.Children.Add(starSEL);

            movePoint = new Point3D(0,0,0);
            rotaPoint = new Point3D(0,0,0);

            starCanvas.MouseLeftButtonDown += SetSelection;
            starCanvas.MouseRightButtonDown +=TargetSelection;

            starCanvas.MouseEnter += ShowInfoSelection;
            starCanvas.MouseLeave += HideInfoSelection;
        }

        public KinectV1Settings(string uniqueID, int kinectNumber)
        {
            uniqueKinectID = uniqueID;
            kinectID = kinectNumber;

            //Set everything to the default value
            colorImageMode = ColorImageFormat.RgbResolution640x480Fps30;
            lineFrequency = PowerLineFrequency.SixtyHertz;
            autoWhiteBalance = true;
            autoExposure = true;
            backlightMode = BacklightCompensationMode.AverageBrightness;
            depthImageMode = DepthImageFormat.Resolution320x240Fps30;
            isNearMode = false;
            irON = true;
            scaleDepthToReliableRange = false;
            colorizeDepth = false;
            mergeSkeletons = false;
            kinectPosition = new Point3D(0, 0, 0);
            kinectYaw = 0.0;
            sendAcceleration = false;
            sendAudioAngle = false;
            audioTrackMode = AudioTrackingMode.Loudest;
            sendColorImage = false;
            sendDepthImage = false;

            //Setup the options for the raw skeleton, irrespective of use
            rawSkeletonSettings = new SkeletonSettings();
        }

        public static Point3D[] GetPoints(int count, Point3D[] controlPoints)
        {
            #region asserts
#if DEBUG
            if (controlPoints.Length < 2)
            {
                throw new ArgumentException("There must be at least two points passed in: " + controlPoints.Length.ToString());
            }
#endif
            #endregion

            double countD = count - 1;

            Point3D[] retVal = new Point3D[count];

            retVal[0] = controlPoints[0];
            retVal[count - 1] = controlPoints[controlPoints.Length - 1];

            for (int cntr = 1; cntr < count - 1; cntr++)
            {
                retVal[cntr] = GetPoint(cntr / countD, controlPoints);
            }

            return retVal;
        }

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

        public ExplodingMesh(MeshGeometry3D inputMesh, Point3D hitpos)
        {
            var mesh = MeshGeometryHelper.NoSharedVertices(inputMesh);

            double cx, cy, cz;
            cx = cy = cz = 0;
            for (int i = 0; i < mesh.Positions.Count; i++)
            {
                cx += mesh.Positions[i].X;
                cy += mesh.Positions[i].Y;
                cz += mesh.Positions[i].Z;
            }
            int n = mesh.Positions.Count;
            var center = new Point3D(cx / n, cy / n, cz / n);

            integrator = new VerletIntegrator();
            integrator.Resize(mesh.Positions.Count);
            var r = new Random();
            for (int i = 0; i < mesh.Positions.Count; i++)
            {
                var delta = mesh.Positions[i] - center;
                delta.Normalize();
                integrator.Positions[i] = mesh.Positions[i] + delta * (1 + r.NextDouble() * 2);
                integrator.Positions0[i] = mesh.Positions[i];
                integrator.Accelerations[i] = new Vector3D(0, 0, -1000);
                integrator.InverseMass[i] = 0.01;
            }

            integrator.CreateConstraintsByMesh(mesh, 0.7);
            integrator.AddFloor(0.3);
            this.Mesh = mesh;
            watch.Start();
        }

 public JointStatus()
 {
     position = new Point3D(0, 0, 0);
     speed = new Point3D(0, 0, 0);
     abs_speed = 0;
     angle = 0;
 }

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

        public static void drawTor(Point3D center, double R, double r, int N, int n, Color color, Viewport3D mainViewport)
        {
            if (n < 2 || N < 2)
              {
            return;
              }

              Model3DGroup tor = new Model3DGroup();
              Point3D[,] points = new Point3D[N, n];

              for (int i = 0; i < N; i++)
              {
            for (int j = 0; j < n; j++)
            {
              points[i, j] = getPositionTor(R, r, i * 360 / (N - 1), j * 360 / (n - 1));
              points[i, j] += (Vector3D)center;
            }
              }

              Point3D[] p = new Point3D[4];
              for (int i = 0; i < N - 1; i++)
              {
            for (int j = 0; j < n - 1; j++)
            {
              p[0] = points[i, j];
              p[1] = points[i + 1, j];
              p[2] = points[i + 1, j + 1];
              p[3] = points[i, j + 1];
              drawTriangle(p[0], p[1], p[2], color, mainViewport);
              drawTriangle(p[2], p[3], p[0], color, mainViewport);

            }
              }
        }

        public FlipTile(DiffuseMaterial frontMaterial,
            Size size, Point center, Material backMaterial, Rect backTextureCoordinates)
        {
            m_locationDesired = new Point3D(center.X, center.Y, 0);
            m_locationCurrent = new Point3D(0, 0, Util.Rnd.NextDouble() * 10 - 20);
            m_size = size;

            Point3D topLeft = new Point3D(-size.Width / 2, size.Height / 2, 0);
            Point3D topRight = new Point3D(size.Width / 2, size.Height / 2, 0);
            Point3D bottomLeft = new Point3D(-size.Width / 2, -size.Height / 2, 0);
            Point3D bottomRight = new Point3D(size.Width / 2, -size.Height / 2, 0);

            m_frontMaterial = frontMaterial;

            Model3DGroup quad = new Model3DGroup();
            quad.Children.Add(
                CreateTile(
                    frontMaterial,
                    backMaterial,
                    m_borderMaterial,
                    new Size3D(size.Width, size.Height, .01),
                    backTextureCoordinates));

            Transform3DGroup group = new Transform3DGroup();

            group.Children.Add(new RotateTransform3D(m_verticalFlipRotation));
            group.Children.Add(new RotateTransform3D(m_quaternionRotation3D));

            group.Children.Add(m_scaleTransform);
            group.Children.Add(m_translate);

            quad.Transform = group;

            this.Visual3DModel = quad;
        }

        public YPitchBendCube(Point3D center, double radius, 
                Pitch pitch, Instrument instrument, OutputDevice device, Channel channel)
            : base(center, radius, new InstrumentNoteAction(device, channel, pitch)) {

                outputDevice = device;
                this.channel = channel;
        }

        /// <summary>
        /// Transform a point
        /// </summary>
        /// <param name="inPoint">Input point</param>
        /// <param name="result">Output point</param>
        /// <returns>True if the point was transformed successfuly, false otherwise</returns>
        public bool TryTransform(Point inPoint, out Point3D result)
        {
            Point final2DPoint;

            // assign this now so that we can return false if needed
            result = new Point3D();
            
            if (!_transform2D.TryTransform(inPoint, out final2DPoint))
            {
                return false;
            }
                       
            Point texCoord = Viewport2DVisual3D.VisualCoordsToTextureCoords(final2DPoint, _childBounds);

            // need to walk the texture coordinates on the Viewport2DVisual3D
            // and look for where this point intersects one of them
            Point3D coordPoint;
            if (!Viewport2DVisual3D.Get3DPointFor2DCoordinate(texCoord, 
                                                            out coordPoint,
                                                            _positions,
                                                            _textureCoords,
                                                            _triIndices))
            {
                return false;
            }

            if (!_transform3D.TryTransform(coordPoint, out result))
            {
                return false;
            }
            
            return true;
        }

        /// <summary>
        /// iterates through the list and averages factor many points into one super point
        /// </summary>
        /// <param name="input">List(Point3D)</param>
        /// <param name="factor">int</param>
        /// <returns>List(Point3D)</returns>
        public static List<Point3D> averageSample(List<Point3D> input, int factor)
        {
            List<Point3D> output = new List<Point3D>();
            for (int i = 0; i < input.Count; i++)
            {
                List<Point3D> sample = new List<Point3D>();
                while ((i < input.Count) && (i % factor != factor - 1))
                {
                    sample.Add(input[i]);
                    i++;
                }

                double aX = 0;
                double aY = 0;
                double aZ = 0;
                for (int j = 0; j < sample.Count; j++)
                {
                    aX = aX + sample[j].X;
                    aY = aY + sample[j].Y;
                    aZ = aZ + sample[j].Z;
                }
                aX = aX / sample.Count;
                aY = aY / sample.Count;
                aZ = aZ / sample.Count;

                Point3D average = new Point3D(aX,aY,aZ);
                output.Add(average);
            }
            return output;
        }

        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 void DebugTracking(Point3D position, int leftSpeed, int rightSpeed, string message)
    {
#if DEBUG_TRACKING
      Console.WriteLine(message + " %(" + leftSpeed + ", " + rightSpeed + ") @(" +
        Math.Round(position.X, 2) + ", " + Math.Round(position.Y, 2) + ", " + Math.Round(position.Z, 2) + ")\n");
#endif
    }

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

        /// <summary>
        /// Get a single point along the curve
        /// </summary>
        /// <returns>
        /// Got this here:
        /// http://www.cubic.org/docs/bezier.htm
        /// </returns>
        public static Point3D GetPoint(double percent, Point3D[] controlPoints)
        {
            #region asserts
#if DEBUG
            if (controlPoints.Length < 2)
            {
                throw new ArgumentException("There must be at least two points passed in: " + controlPoints.Length.ToString());
            }
#endif
            #endregion

            Point3D[] prev = controlPoints;
            Point3D[] current = null;

            for (int outer = controlPoints.Length - 1; outer > 0; outer--)
            {
                current = new Point3D[outer];

                for (int inner = 0; inner < outer; inner++)
                {
                    current[inner] = Math3D.LERP(prev[inner], prev[inner + 1], percent);
                }

                prev = current;
            }

            return current[0];      // by the time execution gets here, the array only has one element
        }