C# Vector3d.Normalize方法代码示例

        public static List<ISectData> IntersectObjects(Vector3d direction, Point3d origin, List<Object3d> objects, bool supports)
        {
            //List<ISectData> m_isectlst = new List<ISectData>();

            try
            {
                if (!vecinit)
                {
                    Initvecs();
                }
                m_isectlst.Clear();
                direction.Normalize();
                direction.Scale(10000.0f);

                IOendp.Set(origin);
                IOendp.x += direction.x;
                IOendp.y += direction.y;
                IOendp.z += direction.z;
                lock (lck)
                {
                    foreach (Object3d obj in objects)
                    {
                        if (obj.tag == Object3d.OBJ_SUPPORT && !supports)
                            continue;
                        // try a less- costly sphere intersect here
                        if (IntersectSphere(origin, IOendp, ref IOintersect, obj.m_center, obj.m_radius))
                        {
                            foreach (Polygon p in obj.m_lstpolys)
                            {
                                //IOintersect = new Point3d();
                                // try a less- costly sphere intersect here
                                if (IntersectSphere(origin, IOendp, ref IOintersect, p.m_center, p.m_radius))
                                {
                                    // if it intersects,
                                    if (RTUtils.IntersectPoly(p, origin, IOendp, ref IOintersect))
                                    {
                                        m_isectlst.Add(new ISectData(obj, p, IOintersect, origin, direction));
                                    }
                                }
                            }
                        }
                    }
                }
                ISectData gp = ISectGroundPlane(direction, origin);
                if (gp != null)
                {
                    m_isectlst.Add(gp);
                }
                m_isectlst.Sort();
            }
            catch (Exception ex)
            {
                DebugLogger.Instance().LogError(ex.Message);
            }
            return m_isectlst;
        }

        /*
        public class Config
        {
            int xres, yres;
           // double
        }
         * */
        public static bool FindIntersection(Vector3d direction, Point3d origin, ref Point3d intersect)
        {
            UVDLPApp.Instance().CalcScene();
            //bool intersected = false;

              //  Point3d bpoint, tpoint;
              //  Point3d lowest = new Point3d(); // the lowest point of intersection on the z axis
            direction.Normalize();
            direction.Scale(100.0);
            Point3d endp = new Point3d();
            endp.Set(origin);
            endp.x += direction.x;
            endp.y += direction.y;
            endp.z += direction.z;
            /*
            intersect = new Point3d();
            intersect.x = 0.0d;
            intersect.y = 0.0d;
            intersect.z = 0.0d;
            */
            //intersect the scene with a ray

               // intersected = false;
            foreach (Polygon p in UVDLPApp.Instance().Scene.m_lstpolys)
            {
                intersect = new Point3d();
                // try a less- costly sphere intersect here
                if (RTUtils.IntersectSphere(origin, endp, ref intersect, p.m_center, p.m_radius))
                {
                    // if it intersects,
                    if (RTUtils.IntersectPoly(p, origin, endp, ref intersect))
                    {
                        return true;
                        /*
                        // and it's the lowest one
                        if (intersect.z <= lowest.z)
                        {
                            //save this point
                            intersected = true;
                            lowest.Set(intersect);
                        }
                         * */
                    }
                }
            }

            return false;
        }

        private static ISectData ISectGroundPlane(Vector3d direction, Point3d origin)
        {
            ISectData isect = null;
            direction.Normalize();
            direction.Scale(10000.0f);

            GPendp.Set(origin);
            GPendp.x += direction.x;
            GPendp.y += direction.y;
            GPendp.z += direction.z;
            // intersect with the imaginary groundplane object;
            if (m_gp == null) 
            {
                CreateGroundPlane();
            }
            if (IntersectSphere(origin, GPendp, ref GPintersect, m_gp.m_center, m_gp.m_radius))
            {
                foreach (Polygon p in m_gp.m_lstpolys)
                {
                    //GPintersect = new Point3d();
                    // try a less- costly sphere intersect here   
                    if (IntersectSphere(origin, GPendp, ref GPintersect, p.m_center, p.m_radius))
                    {
                        // if it intersects,
                        if (RTUtils.IntersectPoly(p, origin, GPendp, ref GPintersect))
                        {
                           isect = new ISectData(m_gp, p, GPintersect, origin, direction);
                        }
                    }
                }
            }
            return isect;
        }

        /// <summary>
        /// For now this is the editing mode for the currently selected support
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void glControl1_Click(object sender, EventArgs e)
        {
            // single click on GL Control
            Object3d obj = UVDLPApp.Instance().SelectedObject;
            if (obj == null) return;
            if (ctrldown == false) return; // ctrl need to be held down

            // this object is a support
            if (obj.tag == Object3d.OBJ_SUPPORT) 
            {
                Support sup = (Support)obj;// we can cast safely
                // now we have to see if we clicked on an object
                MouseEventArgs me = e as MouseEventArgs;
                MouseButtons buttonPushed = me.Button;
                int xPos = me.X;
                int yPos = me.Y;
                List<ISectData> isects = TestHitTest(xPos, yPos);
                if (isects.Count == 0) return; // no intersections
                ISectData isd1 =null;
                foreach (ISectData isd in isects) 
                {
                    // find the closest object we clicked
                    if (isd.obj.tag == Object3d.OBJ_NORMAL) 
                    {
                        isd1 = isd; //  save it
                        break;
                    }
                }
                if (isd1 == null) return; // no object intersection
                isd1.poly.CalcNormal();
                m_isectnormal.x = isd1.poly.m_normal.x;
                m_isectnormal.y = isd1.poly.m_normal.y;
                m_isectnormal.z = isd1.poly.m_normal.z;
                // ok, we've got the normal, we know where we've intersected
                // my best guess is that we should move the support 5mm in the direction of the camera
                // the tip of the support should touch the intersection point
                // let's start with scaling the height...
                //sup.ScaleToHeight(isd1.intersect.z);
                //m_camera.m_eye
                Engine3D.Vector3d towardseye = new Engine3D.Vector3d();
                towardseye = m_isectnormal;// -m_camera.m_eye;
                towardseye.Normalize(); // make the unit length of 1
                towardseye.Scale(4.0f); // scale to 5 mm
                sup.MoveFromTip(isd1.intersect, towardseye);
                UpdateView();
                //sup.
            }
        }

 public void RotateUp(float deg)
 {
     Rotate(m_right, deg);
     // update target and up
     m_target = m_lookat - m_eye;    // The "look-at" unit vector.
     m_target.Normalize();
     m_up = Vector3d.cross(m_right, m_target);
     UpdateView();
 }

        public void ResetView(float x, float y, float z, float updeg, float lookz)
        {
            m_eye = new Vector3d(x, y, z);
            m_lookat = new Vector3d(0, 0, 0);
            m_up = new Vector3d(0, 0, 1);
            m_dz = lookz;
            m_dx = m_dy = 0;
            //m_right = new Vector3d(1, 0, 0);
            m_target = m_lookat - m_eye;    // The "look-at" unit vector.
            m_target.Normalize();
            m_right = Vector3d.cross(m_target, m_up);
            RotateUp(updeg);
            UpdateView();

            //Vector3d xaxis = Vector3d.cross(up, zaxis);// The "right" vector.
            //xaxis.Normalize();
            //Vector3d yaxis = Vector3d.cross(zaxis, xaxis);     // The "up" vector.
        }

        private static ISectData ISectObjSelPlane(Vector3d direction, Point3d origin)
        {
            ISectData isect = null;
            if (m_selplane == null)
                return null;
            direction.Normalize();
            direction.Scale(10000.0f);

            ObSelendp.Set(origin);
            ObSelendp.x += direction.x;
            ObSelendp.y += direction.y;
            ObSelendp.z += direction.z;
            // intersect with the imaginary object selection plane
            if (IntersectSphere(origin, ObSelendp, ref ObSelintersect, m_selplane.m_center, m_selplane.m_radius))
            {
                foreach (Polygon p in m_selplane.m_lstpolys)
                {
                    // try a less- costly sphere intersect here
                    if (IntersectSphere(origin, ObSelendp, ref ObSelintersect, p.m_center, p.m_radius))
                    {
                        // if it intersects,
                        if (RTUtils.IntersectPoly(p, origin, ObSelendp, ref ObSelintersect))
                        {
                            isect = new ISectData(m_selplane, p, ObSelendp, origin, direction);
                        }
                    }
                }
            }
            return isect;
        }

        public static bool IntersectSphere(Point3d start,Point3d end,ref Point3d intersect, Point3d center,double radius)
        {
            bool retval = false;
            double EO;//EO is distance from start of ray to center of sphere
            double d,disc,v;//v is length of direction ray
            Vector3d V,temp;//V is unit vector of the ray
            temp =new Vector3d();
            V = new Vector3d();

            temp.Set(center.x - start.x,center.y - start.y,	center.z - start.z,0);

            EO = temp.Mag(); // unnormalized length
            V.Set(end.x - start.x,end.y - start.y,end.z - start.z,0);
            v = V.Mag();// magnitude of direction vector
            V.Normalize();// normalize the direction vector
            disc = (radius*radius) - ((EO*EO) - (v*v));
            if(disc < 0.0f)
            {
                retval = false;// no intersection
            }
            else
            { // compute the intersection point
                retval = true;
                d = Math.Sqrt(disc);
                intersect.x = start.x + ((v-d)*V.x);
                intersect.y = start.y + ((v-d)*V.y);
                intersect.z = start.z + ((v-d)*V.z);
            }
            return retval;
        }

        /// <summary>
        /// This functiuon positions the base at the location 'intersect'
        /// The idir is the direction that was used to intersect
        /// </summary>
        /// <param name="intersect"></param>
        /// <param name="idir"></param>
        /// <param name="inorm"></param>
        public void PositionBottom(ISectData dat)
        {
            // the bottom could be a tip or base
            // need to orient the bottom and position it.
            Point3d center;
            // for a base support, just slide it around
            if (m_subtype == eSubType.eBase)
            {
                center = Centroid(); // get the centroid of the selected portion of the object
                MinMax mm = CalcMinMaxRange(s1i, s4i);
                float dist = (float)((mm.m_max - mm.m_min) / 2);
                Translate(
                    (float)(dat.intersect.x - center.x),
                    (float)(dat.intersect.y - center.y),
                    (float)(dat.intersect.z - center.z + dist));
            }
            else if (m_subtype == eSubType.eIntra)  // bottom tip
            {
                // for a base tip, find the angle and angle it in
                Vector3d isectnorm = new Vector3d();
                //save the polygon intersection normal
                isectnorm.x = dat.poly.m_normal.x;
                isectnorm.y = dat.poly.m_normal.y;
                isectnorm.z = dat.poly.m_normal.z;
                isectnorm.Normalize();

                if (isectnorm.z < 0)
                {
                    isectnorm.z *= -1.0f;
                }
                // limit the z down to 45 degrees
                if (isectnorm.z < .75)
                {
                    isectnorm.z = .75f;
                }

                // re-genrate the points on the bottom of the foot
                ReGenSegmentPoints(mSC.htrad, mSC.cdivs, s1i, 0, true); // bottom of foot is the tip radius
                Matrix3D tMat = new Matrix3D();
                Vector3d vup = new Vector3d(0, 1, 0);
                Vector3d dir = new Vector3d(isectnorm.x, isectnorm.y, isectnorm.z);
                dir.Normalize();

                //direction should be upward at this point.
                //create a matrix transform
                tMat.LookAt(dir, vup);
                //transform the si1-s2i points to look at the vector
                TransformRange(tMat, s1i, s2i);
                //move the range of points to be touching the intersection point
                STranslateRange(dat.intersect.x, dat.intersect.y, dat.intersect.z, s1i, s2i);

                //now, get the center of s4i to s5i
                Point3d cnt = CalcCentroid(s2i, s4i);
                //translate to 0,0,0
                STranslateRange(-cnt.x, -cnt.y, -cnt.z, s2i, s4i);
                //reset the points,
                ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s2i, 0, false); // top of foot
                ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s3i, 0, false); // top of foot
                Point3d newp = new Point3d();
                newp.x = dat.intersect.x + (isectnorm.x * 2);
                newp.y = dat.intersect.y + (isectnorm.y * 2);
                newp.z = dat.intersect.z + (isectnorm.z * 2);
                STranslateRange(newp.x,newp.y,newp.z, s2i, s4i);

                Update();
            }
            //Update();
        }

        public void MoveFromTip(ISectData dat)
        {
            Point3d center;
            center = Centroid(); // get the centroid of the selected portion of the object
            Vector3d isectnorm = new Vector3d();
            //save the polygon intersection normal
            isectnorm.x = dat.poly.m_normal.x;
            isectnorm.y = dat.poly.m_normal.y;
            isectnorm.z = dat.poly.m_normal.z;

            MinMax mm = CalcMinMaxRange(s4i, m_lstpoints.Count);
            float dist = (float)((mm.m_max - mm.m_min) );
            ResetTip();
            Matrix3D tMat = new Matrix3D();
            Vector3d vup = new Vector3d(0,1,0);
            //always make sure the z is heading downward
            if (isectnorm.z > 0)
            {
                isectnorm.z *= -1.0f;
            }
            // limit the z down to 45 degrees
            if (isectnorm.z > -.75)
            {
                isectnorm.z = -.75f;
            }
            //reverse the direction to get the reflection
            Vector3d dir = new Vector3d(-isectnorm.x, -isectnorm.y, -isectnorm.z);
            dir.Normalize();
            //create a matrix transform
            tMat.LookAt(dir, vup);
            Transform(tMat);
            Translate(
                (float)(dat.intersect.x),
                (float)(dat.intersect.y),
                (float)(dat.intersect.z));

            //now, get the center of s4i to s5i
            Point3d cnt = CalcCentroid(s4i, s5i);
            //translate to 0
            TranslateRange(-cnt.x, -cnt.y, -cnt.z, s4i, s5i);
            //reset the points,
            ReGenSegmentPoints(mSC.hbrad, mSC.cdivs, s4i, 0, false); // bottom of head
            //move back
            TranslateRange(cnt.x, cnt.y, cnt.z, s4i, s5i);
            Update();

            //
        }

        // split edge v1-v2. v3 is the last corner in the triangle and is used for computing normals
        int SplitEdge(int v1, int v2, Vector3d norm1, Vector3d norm2, out Vector3d norm12)
        {
            EdgeAmf edge = m_pointList[v1].FindEdge(v2);
            if (edge == null)
            {
                edge = m_pointList[v2].FindEdge(v1);
                if (edge != null)
                {
                    // swap verteces to match edge
                    int tv = v1;
                    v1 = v2;
                    v2 = tv;
                    Vector3d tnorm = norm1;
                    norm1 = norm2;
                    norm2 = tnorm;
                }
            }

            Vector3d t1, t2;
            PointAmf pamf1 = m_pointList[v1];
            PointAmf pamf2 = m_pointList[v2];
            Point3d pt1 = pamf1.pt;
            Point3d pt2 = pamf2.pt;
            PointAmf pamf;
            float x, y, z;

            // calculate edge vector
            x = pt2.x - pt1.x;
            y = pt2.y - pt1.y;
            z = pt2.z - pt1.z;
            Vector3d edgeDir = new Vector3d(x, y, z);

            // first see if we have an edge for this segment
            if (edge != null)
            {
                // if this edge was already split, return result
                if (edge.v12 >= 0)
                {
                    norm12 = CalcCenterNormal(norm1, norm2, edge.t12);
                    return edge.v12;
                }
                t1 = edge.t1;
                t2 = edge.t2;
            }
             else
            {
                t1 = GetTangetFromNormal(norm1, edgeDir);
                t2 = GetTangetFromNormal(norm2, edgeDir);
            }

            float d = edgeDir.Mag();

            // calculate mid point using Hermite interpolation
            x = 0.5f * pt1.x + 0.125f * t1.x * d + 0.5f * pt2.x - 0.125f * t2.x * d;
            y = 0.5f * pt1.y + 0.125f * t1.y * d + 0.5f * pt2.y - 0.125f * t2.y * d;
            z = 0.5f * pt1.z + 0.125f * t1.z * d + 0.5f * pt2.z - 0.125f * t2.z * d;

            pamf = new PointAmf();
            pamf.pt = new Point3d(x, y, z);
            int v = m_pointList.Count;
            m_pointList.Add(pamf);

            // calculate new tanget and new normal
            x = -1.5f * pt1.x - 0.25f * t1.x * d + 1.5f * pt2.x - 0.25f * t2.x * d;
            y = -1.5f * pt1.y - 0.25f * t1.y * d + 1.5f * pt2.y - 0.25f * t2.y * d;
            z = -1.5f * pt1.z - 0.25f * t1.z * d + 1.5f * pt2.z - 0.25f * t2.z * d;
            Vector3d tanget = new Vector3d(x, y, z);
            tanget.Normalize();

            norm12 = CalcCenterNormal(norm1, norm2, tanget);
            
            if (edge == null)
            {
                //pamf.normal = GetNormalFromTanget(norm1, tanget);
                // create an edge for this segment
                edge = new EdgeAmf();
                edge.v1 = v1;
                edge.v2 = v2;
                edge.t1 = t1;
                edge.t2 = t2;
                pamf1.AddEdge(edge);
            }
            edge.t12 = tanget;
            edge.v12 = m_pointList.Count - 1; // saves double computation 

            //tanget.Normalize();
            // save 2 split edges
            EdgeAmf edge1 = new EdgeAmf();
            edge1.v1 = v1;
            edge1.v2 = v;
            edge1.t1 = t1;
            edge1.t2 = tanget;
            pamf1.AddEdge(edge1);

            EdgeAmf edge2 = new EdgeAmf();
            edge2.v1 = v;
            edge2.v2 = v2;
            edge2.t1 = tanget;
            edge2.t2 = t2;
//.........这里部分代码省略.........