inline Wvec
kernel_vec(const WMat3& M)
{
   // return a vector perpendicular to all 3 rows

   // only supposed to call this on a singular matrix
   if (fabs(M.det()) > 1e-5) {
      cerr << "kernel_vec: warning: matrix is not singular:"
           << endl
           << M
           << endl
           << "determinant: "
           << M.det()
           << endl;
      return Wvec();
   }

   // get row vectors, changed to unit length or null:
   Wvec r0 = M.row(0).normalized();
   Wvec r1 = M.row(1).normalized();
   Wvec r2 = M.row(2).normalized();

   // re-order to push null ones to the end:
   if (r0.is_null()) swap(r0,r1);
   if (r0.is_null()) swap(r0,r2);
   if (r1.is_null()) swap(r1,r2);

   Wvec ret = cross(r0,r1).normalized();
   if (ret.is_null())
      ret = cross(r0,r2).normalized();
   if (ret.is_null())
      ret = Wvec::X();
   assert(isZero(ret*r0) && isZero(ret*r1) && isZero(ret*r2));
   return ret;
}

//! Given a set of enclosed face, activate the widget to sweep out a
//! shape. Checks for errors, returns true on success.
bool
SWEEP_DISK::setup(CGESTUREptr& gest, double dur)
{

   static bool debug =
      Config::get_var_bool("DEBUG_SWEEP_SETUP",false) || debug_all;

   if (!(gest && gest->is_dslash())) {
      err_adv(debug, "SWEEP_DISK::setup: bad gesture");
      return false;
   }

   // XXX - shouldn't require it is a Panel:
   Panel* p = dynamic_cast<Panel*>(Bsurface::hit_ctrl_surface(gest->start()));
   if (!p) {
      err_adv(debug, "SWEEP_DISK::setup: non-panel");
      return false;
   }

   Bface_list faces = p->bfaces();

   _boundary = faces.get_boundary();
   if (_boundary.num_line_strips() != 1) {
      err_adv(debug, "SWEEP_DISK::setup: error: boundary is not a single piece");
      return false;
   }

   // Get the best-fit plane, rejecting if the boundary Wpt_list
   // doesn't lie within 0.1 of its total length from the plane:
   if (!_boundary.verts().pts().get_plane(_plane, 0.1)) {
      err_adv(debug,"SWEEP_DISK::setup: Error: can't find plane");
      return false;
   }
   
   // Find the center
   Wpt o = _boundary.verts().pts().average();

   // decide guideline direction (normal to plane):
   Wvec n = _plane.normal();
   if (VIEW::eye_vec(o) * n > 0)
      n = -n;

   // decide the length for the guideline:
   double len = world_length(o, GUIDE_LEN);

   // compute guideline endpoint:
   Wpt b = o + n.normalized()*len;

   // try basic setup
   if (!SWEEP_BASE::setup(dynamic_pointer_cast<LMESH>(faces.mesh()), o, b, dur))
      return false;

   // ******** From here on we accept it ********

   _enclosed_faces = faces;

   return true;
}

Wvec
Bface_list::avg_normal() const
{
   // Returns the average of the face normals
   Wvec ret;
   for (Bface_list::size_type i=0; i<size(); i++)
      ret += at(i)->norm();
   return ret.normalized();
}

Wvec
Bface::quad_tan2() const 
{
   // Based on the 4 verts in standard orientation as above,
   // return the tangent vector running up
   Wpt a, b, c, d;
   get_quad_pts(a,b,c,d);

   Wvec t = ((d - a) + (c - b))*0.5;
   return t.orthogonalized(quad_norm()).normalized();
}

void
XToonStripCB::faceCB(CBvert* v, CBface* f)
{
   assert(v && f);
   Wvec bNorm; //Blended Normal

   //first calculate the abstract(blended) normal
   switch(_blend_type) {
    case XToonStripCB::SMOOTH: {
       // Note: doesn't work
       bNorm = v->get_all_faces().n_ring_faces(3).avg_normal();
    }
      break;
    case XToonStripCB::SPHERIC: {
       BMESH* mesh = v->mesh();
       Wpt c = mesh->get_bb().center();
       bNorm = (v->loc()-c).normalized();
    }
      break;
    case XToonStripCB::ELLIPTIC: {
       BMESH* mesh = v->mesh();
       Wvec c_to_v = v->loc() - mesh->get_bb().center();
       Wvec dim = mesh->get_bb().dim();
       double a = dim[0]*0.5;
       double b = dim[1]*0.5;
       double c = dim[2]*0.5;
       bNorm = Wvec(c_to_v[0]/a, c_to_v[1]/b, c_to_v[2]/c).normalized();
    }
      break;
    case XToonStripCB::CYLINDRIC: {
       BMESH* mesh = v->mesh();
       Wpt c = mesh->get_bb().center();
       Wvec axis;
       Wvec dim = mesh->get_bb().dim();
       if (dim[0]>dim[1] && dim[0]>dim[2])
          axis = dim.X();
       else if (dim[1]>dim[0] && dim[1]>dim[2])
          axis = dim.Y();         
       else 
          axis = dim.Z();         
      
       Wpt v_proj = c + ((v->loc()-c)*axis) * axis;
       bNorm = (v->loc()-v_proj).normalized();
    }
      break;
    default:
      assert(0);
   }

   //set the blended normal, the regular normal and the vertex point
   glVertexAttrib3f(_loc, bNorm[0], bNorm[1], bNorm[2]); 
   glNormal3dv(f->vert_normal(v).data());
   glVertex3dv(v->loc().data());
}

NDCpt 
Bface::nearest_pt_ndc(CNDCpt& p, Wvec &bc, int &is_on_tri) const 
{
   // Bsimplex virtual method

   // same as above, but operates in NDC space

   // get barycentric coords:
   NDCpt a = _v1->ndc();
   NDCpt b = _v2->ndc();
   NDCpt c = _v3->ndc();
   double A = signed_area_ndc(a, b, c);
   double u = signed_area_ndc(p, b, c) / A;
   double v = signed_area_ndc(a, p, c) / A;
   bc.set(u, v, 1 - u - v);
   
   // to account for numerical errors, snap
   // near-zero values to 0 and renormalize
   snap(bc);

   if (bc[0] < 0 || bc[1] < 0 || bc[2] < 0) {
      // p is outside the triangle.
      // find closest point to an edge:
      is_on_tri = 0;
      double t1, t2, t3;

      NDCpt p1 = pt_near_seg_ndc(a,b,p,t1);
      NDCpt p2 = pt_near_seg_ndc(b,c,p,t2);
      NDCpt p3 = pt_near_seg_ndc(c,a,p,t3);

      double d1 = p.dist_sqrd(p1);
      double d2 = p.dist_sqrd(p2);
      double d3 = p.dist_sqrd(p3);

      if (d1 < d2) {
         if (d1 < d3) {
            bc.set(1-t1,t1,0);
            return p1;
         }
         bc.set(t3,0,1-t3);
         return p3;
      }
      if (d2 < d3) {
         bc.set(0,1-t2,t2);
         return p2;
      }
      bc.set(t3,0,1-t3);
      return p3;
   }

   is_on_tri = 1;
   return (a*bc[0]) + (b*bc[1]) + (c*bc[2]);
}

double
Collide::intersectSphere(CWpt& rO, CWvec& rV, CWpt& sO, double sR)
{
   Wvec Q = sO - rO;
   double c = Q.length();
   double v = Q * rV;
   double d = sR*sR - (c*c - v*v);

   // If there was no intersection, return -1

   if (d < 0.0) return -1.0;

   // Return the distance to the [first] intersecting point

   return v - sqrt(d);
}

void 
Bpoint::remove_constraining_surface()
{
  if ( !(constraining_surface()) ){
    cerr << "Bpoint::remove_constraining_surface() "
         << "has no surface constraint" << endl;
    return;
  }

  // save the normal
  Wvec n = norm();

  // remove the shadow, if any 
  remove_shadow();
  set_map(new WptMap(loc()), false);

  if (!n.is_null())
    _map->set_norm(n);
}

Wpt
Bedge::nearest_pt(CWpt& p, Wvec &bc, bool &is_on_simplex) const
{
    Wvec ab = _v2->loc() - _v1->loc();
    Wvec ac = p - _v1->loc();

    double dot = (ab * ac) / ab.length_sqrd();
    bc.set(1-dot, dot, 0);

    if (dot < gEpsZeroMath) {
        bc.set(1, 0, 0);
        is_on_simplex = (dot >= 0);
    } else if (1-dot < gEpsZeroMath) {
        bc.set(0, 1, 0);
        is_on_simplex = (dot <= 1);
    }

    return (bc[0] * _v1->loc()) + (bc[1] * _v2->loc());
}

inline void
add_shading(CBvert_list& verts, Wvec l, CCOLOR& col, double s = 1.0)
{
   // normalize the "light" vector:

   l = l.normalized();
   for (size_t i=0; i<verts.size(); i++) {
      double a = pow(max(l * verts[i]->norm(), 0.0), s);
      if (a > 0)
         verts[i]->set_color(interp(verts[i]->color(), col, a), 1);
   }
}

void
CIRCLE_WIDGET::make_preview( void )
{
   _preview.clear();

   // Get a coordinate system
   Wvec Z = _plane.normal();
   Wvec X = Z.perpend();
   Wvec Y = cross(Z,X);
   Wtransf xf(_center, X, Y, Z);

   // Make the hi-res circle for the curve's map1d3d:
   const int ORIG_RES = 256;
   _preview.realloc(ORIG_RES + 1);
   double dt = (2*M_PI)/ORIG_RES;
   for (int i=0; i<ORIG_RES; i++) {
      double t = dt*i;
      _preview += xf*Wpt(_radius*cos(t), _radius*sin(t), 0);
   }
   _preview += _preview[0];       // make it closed

   if( _suggest_active ) {
      return;
   }

   if( _circle == 0 ) {
      // XXX - no undo! should fix
      _circle = PanelAction::create(
         _plane, _center, _radius, TEXBODY::get_skel_mesh(0), _disk_res, 0
         );
   } else {
      Bcurve *border = Bcurve::lookup(_circle->bfaces().get_boundary().edges());
      if( border != 0 ) {
         Wpt_listMap *map = Wpt_listMap::upcast(border->map());
         if( map )
            map->set_pts(_preview);
      }
   }

}

/**********************************************************************
 * NPRSolidTexCB:
 **********************************************************************/
void 
NPRSolidTexCB::faceCB(CBvert* v, CBface*f) 
{
   Wvec n;
   f->vert_normal(v,n);

   if (!nst_use_vertex_program)
   {
      if (nst_tex_flag) {
         TexCoordGen* tg = f->patch()->tex_coord_gen();
         if (tg) 
            glTexCoord2dv(tg->uv_from_vert(v,f).data());
         else if (UVdata::lookup(f))
            glTexCoord2dv(UVdata::get_uv(v,f).data());
      }
   
      if (nst_paper_flag)
         PaperEffect::paper_coord(NDCZpt(v->wloc()).data());
      
      glNormal3dv(n.data());
      glVertex3dv(v->loc().data());
   }
   else
   {
      if (nst_tex_flag)
      {
         TexCoordGen* tg = f->patch()->tex_coord_gen();
         if (tg) 
            glTexCoord2dv(tg->uv_from_vert(v,f).data());
         else if (UVdata::lookup(f))
            glTexCoord2dv(UVdata::get_uv(v,f).data());
      }


      glNormal3dv(n.data());
      glVertex3dv(v->loc().data());
   }

}

CWpt& 
SkinMeme::compute_update()
{
   static bool debug = ::debug || Config::get_var_bool("DEBUG_SKIN_UPDATE",false);

   // compute 3D vertex location WRT track simplex

   if (_is_sticky) {
      // this meme is supposed to follow the skeleton surface
      if (is_tracking()) {
         // it actually is following it
         return _update = skin_loc(track_simplex(), _bc, _h);
      }
      // supposed to follow, but has no track point: do nothing
      return _update = loc();
   }

   // this meme is not following the skeleton surface;
   // it computes its location via smooth subdivision.
   // but it may still track the closest point on the skeleton
   // surface to avoid penetrating inside the skeleton surface.

   if (vert()->parent() == 0)
      _update = loc();
   else
      _update = vert()->detail_loc_from_parent();
   track_to_target(_update);
   if (_non_penetrate && is_tracking()) {
      Wvec d = penetration_correction(_update, track_simplex(), _bc, _stay_out);
      if (debug && !d.is_null())
         err_msg("SkinMeme::compute_update: correcting penetration, level %d",
                 bbase()->subdiv_level());
      _update += d;
               
   }
   return _update;
}

bool 
SWEEP_LINE::create_rect(CWvec& v)
{
   // create a rectangular Panel based on given vector along the guideline

   //   Get oriented as follows, looking down onto the plane:
   //                                                       
   //      b1 . . . . . . . b4                              
   //      |                 .                              
   //      |                 .                              
   //      |                 .                              
   //      | ------- v ----->.                              
   //      |                 .                              
   //      |                 .                              
   //      |                 .                              
   //      b2 . . . . . . . b3                              

   static bool debug =
      Config::get_var_bool("DEBUG_CREATE_RECT",false) || debug_all;

   assert(_curve != nullptr);
   Bpoint *b1 = _curve->b1(), *b2 = _curve->b2();
   assert(b1 && b2);
   Wvec u = b2->loc() - b1->loc();      // vector along existing straight line

   // Swap b1 and b2 if necessary:
   Wvec n = _plane.normal();
   if (det(v,n,u) < 0) {
	  err_adv(debug, "SWEEP_LINE::create_rect: b1 and b2 swapped");
      //swap(b1,b2);
      //u = -u;
   }

   // Decide number of edges "horizontally" (see diagram above)
   int num_v = _curve->num_edges(); // number of edges "vertically"
   double H = u.length();           // "height"
   double W = v.length();           // "width"
   double l = H/num_v;              // length of an edge "vertically"
   int num_h = (int)round(W/l);     // number of edges "horizontally"
   if (num_h < 1) {
      // Needs more work to handle this case. Bail for now:
      err_adv(debug, "SWEEP_LINE::create_rect: cross-stroke too short");
      return false;
   }

   // Accept it now

   LMESHptr m = _curve->mesh();
   Wpt p1 = b1->loc(), p2 = b2->loc(), p3 = p2 + v, p4 = p1 + v;

   MULTI_CMDptr cmd = make_shared<MULTI_CMD>();

   // Create points b3 and b4
   Bpoint* b3 = BpointAction::create(m, p3, n, v, b2->res_level(), cmd);
   Bpoint* b4 = BpointAction::create(m, p4, n, v, b1->res_level(), cmd);

   // Create the 3 curves: bottom, right and top
   Wpt_list side;
   int res_lev = _curve->res_level();

   err_adv(debug, "SWEEP_LINE::create_rect: curve res level: %d", res_lev);

   Bcurve_list contour;
   contour += _curve;

   // Bottom curve
   side.clear(); side.push_back(p2); side.push_back(p3);
   contour += BcurveAction::create(m, side, n, num_h, res_lev, b2, b3, cmd);

   // Right curve
   side.clear(); side.push_back(p3); side.push_back(p4);
   contour += BcurveAction::create(m, side, n, num_v, res_lev, b3, b4, cmd);

   // Top curve
   side.clear(); side.push_back(p4); side.push_back(p1);
   contour += BcurveAction::create(m, side, n, num_h, res_lev, b4, b1, cmd);

   // Interior
   PanelAction::create(contour, cmd);
   
   WORLD::add_command(cmd);
   
   return true;
}

 static void clamp_barycentric(Wvec &bc) {
    bc.set(max(bc[0],0.0), max(bc[1],0.0), max(bc[2],0.0));
    bc /= (bc[0] + bc[1] + bc[2]);
 }

/********************************************************
Given a velocity vector and a position, it will test all
objects found with the sps octree for collisions and
return and new velocity that doesn't run through objects
********************************************************/
CWvec
Collide::_get_move(CWpt& s, CWvec& vel)
{
   if (_land == NULL)
      return vel;

   //transform source/velocty to object space
   Wpt source = _land->inv_xform() * s;
   Wvec velocity = _land->inv_xform() * vel;

   Wpt dest = source + velocity; //destination to travel to (obj space)
   double speed = velocity.length();

   _hitFaces.clear();

   double boxsize = _size * 5;
   Wvec d = Wvec(1,1,1)*boxsize;
   _camBox = BBOX(source - d, source + d);

	_hitFaces.clear();

	//build collision list from the land
	 buildCollisionList(_RootNode);
		
	//if(_hitFaces.num() != 0)
	//	cout << "Faces Found: " << _hitFaces.num() << endl;

   //if there are no near by nodes then bring camera closer to the object
   if (_hitFaces.empty())
		{
		Wvec force = _land->bbox().center() - dest;
		return velocity+(_size * .1 * log(force.length()) * force);
		}

   ARRAY<Wvec> norms;
   ARRAY<double> weights;
   double totalWeight = 0;

	//spring forces

   //weight all near by nodes
   for (int i = 0; i < _hitFaces.num(); i++) {
      Wpt p;
      _hitFaces[i]->bc2pos(_smplPoints[i],p);
      Wvec n = _hitFaces[i]->bc2norm(_smplPoints[i]);

      //get the projected distance of the camera and the surface point
      //against the normal of the surface point
      Wvec v  = (dest - p).projected(n);
      double dist = n*v;

      //calculate the weight of given point
      weights.add(pow(e,sqr(dist)));
      totalWeight+=weights[i];

      //calculate normal
      if (dist <= _size)       //if its closer than it should be
         norms += speed * (_size - dist) * n;
      else                            //if its further than should be             
         norms += speed * (_size - dist) * -n;
   }

   //calculate combination of all weighted norms
   Wvec force = Wvec(0,0,0);
   for (int i = 0; i < _hitFaces.num(); i++)
      force += (weights[i]/totalWeight) * norms[i];

	//smooth forces so its not jerky
	double a = .1;
   _prevForce = force;
	force = ((1 - a) * (force - _prevForce)) +_pV;
   _pV = force;

/*   
   for (int i = 0; i < _hitFaces.num(); i++)
		{
		Wpt p;
		_hitFaces[i]->bc2pos(_smplPoints[i],p);
		Wvec n = _hitFaces[i]->bc2norm(_smplPoints[i]);

		Wvec v  = ((source + (velocity + force)) - p).projected(n);
		double dist = n*v;
		if(dist < _size)
			velocity = velocity + (n *(_size - dist));
		}
	*/

   return _land->xform() * (velocity + force);
}

 // Assign a normal explicitly. Note that if vertices are moved,
 // nearby normals will be recomputed by averaging face normals.
 // XXX - does not apply to vertices on creases
 void set_norm(Wvec n) {
    _norm = n.normalized();
    set_bit(VALID_NORMAL_BIT); 
 }

void
LMESH::fit(vector<Lvert*>& verts, bool do_gauss_seidel)
{
   static bool debug = Config::get_var_bool("DEBUG_LMESH_FIT",false);
   static bool move_along_normal =
      Config::get_var_bool("FITTING_MOVE_ALONG_NORMAL",false);

   if (verts.empty())
      return;

   // calculate the bounding box of the vertices
   BBOX box;
   size_t i;
   for (i=0; i<verts.size(); i++)
      box.update(verts[i]->loc());

   double max_err = box.dim().length() * 1e-5;

   size_t n = verts.size();

   // get original control point locations
   vector<Wpt> C(n);   // original control points
   vector<Wpt> L(n);   // current limit points
   for (i=0; i<n; i++) {
      C[i] = verts[i]->loc();
      L[i] = Wpt::Origin();
   }

   if(debug) {
      cerr << "LMESH::fit- fitting " << n << " vertices"<<endl;
      cerr << "Max_err = " << max_err <<endl;
   }

   // do 50 iterations...
   double prev_err = 0;
   vector<double> errors;
   for (int k=0; k<50; k++) {

      errors.clear();

      double err = 0;

      if (do_gauss_seidel) {

         // Gauss-Seidel iteration: use updated values from the
         // current iteration as they are computed...
         for (size_t j=0; j<n; j++) {
            // don't need that L[] array...
            Wpt limit;
            verts[j]->limit_loc(limit);
            Wvec delt = C[j] - limit;
            errors.push_back(delt.length());
            err += delt.length();
            if(move_along_normal)
               delt = delt*verts[j]->norm()*verts[j]->norm();
            verts[j]->offset_loc(delt);
         }

      } else {
         // compute the new offsets from the offsets computed in the
         // previous iteration
         size_t j;
         for (j=0; j<n; j++)
            verts[j]->limit_loc(L[j]);

         for (j=0; j<n; j++) {
            Wvec delt = C[j] - L[j];

            err += delt.length();
            errors.push_back(delt.length());
            if(move_along_normal)
               delt = delt*verts[j]->norm()*verts[j]->norm();
            verts[j]->offset_loc(delt);
         }
      }
      // compute the average error:
      err /= n;

      double avg,std_d,max,min;
      if (debug) {
         if (prev_err != 0) {
            err_msg("Iter %d: avg error: %f, reduction: %f",
                    k, err, err/prev_err);
            statistics(errors,true,&avg,&std_d,&max,&min);
         } else {
            err_msg("Iter %d: avg error: %f", k, err);
            statistics(errors,true,&avg,&std_d,&max,&min);
         }
      } else
         statistics(errors,false,&avg,&std_d,&max,&min);

      prev_err = err;

      if (max < max_err) {
         if(debug) cerr << "Terminating at " << k <<" th iterations"<<endl;
         return;
      }
   }
}

void
Bedge::project_barycentric(CWpt &p, Wvec &bc) const
{
    double t = ((p - _v1->loc()) * vec()) / sqr(length());
    bc.set(1.0 - t, t, 0);
}

//.........这里部分代码省略.........
      double step = 1.0/((double)(num-1));
      for (k=0 ; k<num ; k++) wlScaledList.push_back(wlProjList.interpolate((double)k*step));
   } else { //CURVE_MODE_PLANE
      assert(curve_type == HatchingGroup::CURVE_MODE_PLANE);

      err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed::add() - Resampling curve.");

      //Resample to even spacing in world space. This curve will
      //be sampled on the order of the mesh spacing but we'll
      //not allow the num of samples to drop too low in case
      //the gesture's on the scale of one triangle
      size_t num = max(wlClipList.size(), 5UL);
      double step = 1.0/((double)(num-1));
      for (k=0 ; k<num ; k++) wlScaledList.push_back(wlClipList.interpolate((double)k*step));
   }

   // Convert back to 2D
   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed:add() - converting to 2D.");
   NDCZpt_list ndczlScaledList;
   for (k=0;k<wlScaledList.size();k++) ndczlScaledList.push_back(NDCZpt(_patch->xform()*wlScaledList[k]));
   ndczlScaledList.update_length();

   // Calculate pixel length of hatch
   double pix_len = ndczlScaledList.length() * VIEW::peek()->ndc2pix_scale();

   if (pix_len < 8.0)
   {
      err_mesg(ERR_LEV_WARN, "HatchingGroupFixed::add() - Stroke only %f pixels. Probably an accident. Punting...", pix_len);
      return false;
   }

   vector<HatchingFixedVertex>   verts;
   Wpt_list                      pts;
   vector<Wvec>                  norms;

   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed::add() - Final sampling.");

   for (k=0; k<ndczlScaledList.size(); k++) {
      Wpt wloc;
      f = HatchingGroupBase::find_face_vis(NDCpt(ndczlScaledList[k]),wloc);

      if (f && f->patch() == _patch && f->front_facing()) {
         Wvec bc;
         Wvec norm;

         //f->project_barycentric(wloc,bc);
         f->project_barycentric_ndc(NDCpt(ndczlScaledList[k]),bc);

         Wvec bc_old = bc;
         Bsimplex::clamp_barycentric(bc);
         double dL = fabs(bc.length() - bc_old.length());

         if (bc != bc_old) {
            err_mesg(ERR_LEV_INFO, 
               "HatchingGroupFixed::add() - Baycentric clamp modified result: (%f,%f,%f) --> (%f,%f,%f) Length Change: %f", 
                  bc_old[0], bc_old[1], bc_old[2], bc[0], bc[1], bc[2], dL);
         }
         if (dL < 1e-3) {
            verts.push_back(HatchingFixedVertex(f->index(), bc));
            f->bc2norm_blend(bc,norm);
            pts.push_back(wloc);
            norms.push_back(norm);
         } else {
            err_mesg(ERR_LEV_WARN, "HatchingGroupFixed::add() - Change too large due to error in projection. Dumping point...");
         }