C++ Wvec::length方法代码示例

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

/////////////////////////////////////
// add()
/////////////////////////////////////
bool
HatchingGroupFixed::add(
   CNDCpt_list &pl,
   const vector<double>&prl,
   int curve_type
   )
{
   size_t k;
   double a,b;
   Bface *f;

   // It happens:
   if (pl.empty()) {
      err_mesg(ERR_LEV_ERROR, "HatchingGroupFixed:add() - Error: point list is empty!");
      return false;
   }

   if (prl.empty()) {
      err_mesg(ERR_LEV_ERROR, "HatchingGroupFixed:add() - Error: pressure list is empty!");
      return false;
   }

   if (pl.size() != prl.size()) {
      err_mesg(ERR_LEV_ERROR, "HatchingGroupFixed:add() - gesture pixel list and pressure list are not same length.");
      return false;
   }

   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed:add() - smoothing gesture.");

   //Smooth the input gesture
   NDCpt_list              smoothpts;
   vector<double>          smoothprl;
   if (!smooth_gesture(pl, smoothpts, prl, smoothprl, _params.anim_style()))
      return false;

   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed:add() - clipping gesture to model.");

   NDCpt_list              ndcpts;
   vector<double>          finalprl;
   clip_to_patch(smoothpts, ndcpts, smoothprl, finalprl);
   ndcpts.update_length();

   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed::add() - Checking gesture silliness.");

   if (HatchingGroupBase::is_gesture_silly(ndcpts,_params.anim_style()))
   {
      err_mesg(ERR_LEV_WARN, "HatchingGroupFixed::add() - Punting silly gesture...");
      return false;
   }

   //Even if the user wants to project to create the
   //hatch, we continue with plane cutting to
   //generate a curve we can use to estimate
   //the mesh spacing so that the final projected
   //hatch is sampled evenly on the level of the mesh
   //spacing

   //Get the cutting line
   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed:add() - fitting line.");
   if (!fit_line(ndcpts,a,b)) return false;

   //Slide to midpoint if desired
   if (Config::get_var_bool("HATCHING_GROUP_SLIDE_FIT",false,true)) 
      b = ndcpts.interpolate(0.5)[1] - (a*ndcpts.interpolate(0.5)[0]);

   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed:add() - computing plane.");

   //Find the cutting plane
   Wplane wpPlane;
   f = compute_cutting_plane(_patch, a, b, ndcpts, wpPlane);
   if (!f) return false;
   else
   {
      if (!f->front_facing())
      {
         err_mesg(ERR_LEV_WARN, "HatchingGroupFixed::add() - Nearest pt. on fit line hit backfacing surface.");
         return false;
      }
   }

   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed:add() - slicing mesh.");
   
   //Intersect the mesh to get a 3D curve
   Wpt_list wlList;
   slice_mesh_with_plane(f,wpPlane,wlList);

   err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed:add() - cliping curve to gesture.");

   //Clip end of 3D curve to match gesture
   Wpt_list wlClipList;
   clip_curve_to_stroke(_patch, ndcpts, wlList, wlClipList);
   wlClipList.update_length();

   Wpt_list wlScaledList;

   if (curve_type == HatchingGroup::CURVE_MODE_PROJECT) {
      err_mesg_cond(debug, ERR_LEV_SPAM, "HatchingGroupFixed::add() - Projecting to surface.");
//.........这里部分代码省略.........

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

/********************************************************
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);
}

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
fit(LMESHptr& mesh, bool do_gauss_seidel)
{
   if (mesh->empty())
      return;

   // time this
   stop_watch clock;

   double max_err = mesh->get_bb().dim().length() * 1e-5;
   int n = mesh->nverts();

   // get original control point locations
   Wpt_list C(n);   // original control points
   Wpt_list L(n);   // current limit points
   for (int i=0; i<n; i++) {
      C += mesh->bv(i)->loc();
      L += Wpt::Origin();
   }

   // do 50 iterations...
   double prev_err = 0;
   for (int k=0; k<50; k++) {
      double err = 0;
      if (do_gauss_seidel) {
         // Gauss-Seidel iteration: use updated values from the
         // current iteration as they are computed...
         for (int j=0; j<n; j++) {
            // don't need that L[] array...
            Wpt limit;
            mesh->lv(j)->limit_loc(limit);
            Wvec delt = C[j] - limit;
            err += delt.length();
            mesh->bv(j)->offset_loc(delt);
         }
      } else {
         // compute the new offsets from the offsets computed in the
         // previous iteration
         int j;
         for (j=0; j<n; j++)
            mesh->lv(j)->limit_loc(L[j]);
         for (j=0; j<n; j++) {
            Wvec delt = C[j] - L[j];
            err += delt.length();
            mesh->bv(j)->offset_loc(delt);

         }
      }
      // compute the average error:
      err /= n;
      if (prev_err != 0) {
         err_msg("Iter %d: avg error: %f, reduction: %f",
                 k, err, err/prev_err);
      } else {
         err_msg("Iter %d: avg error: %f", k, err);
      }
      prev_err = err;
      if (err < max_err)
         break;
   }

   err_msg("fitting took %.2f seconds", clock.elapsed_time());
}