C++ Vec3f::dot方法代码示例

//This is the main trace function. It takes a ray as argument (defined by its origin and direction).
//We test if this ray intersects any of the geometry in the scene.
//If the ray intersects an object, we compute the intersection point, the normal at the intersection point,
//and shade this point using this information. Shading depends on the surface property
//(is it transparent, reflective, diffuse). The function returns a color for the ray.
//If the ray intersects an object that is the color of the object at the intersection point,
//otherwise it returns the background color.
Vec3f trace( const Vec3f &rayorig, const Vec3f &raydir,
             const std::vector<Sphere> &spheres, const int &depth)
{
    //if (raydir.length() != 1) std::cerr << "Error " << raydir << std::endl;
    float tnear = INFINITY;
    const Sphere* sphere = NULL;
    // find intersection of this ray with the sphere in the scene
    for (unsigned i = 0; i < spheres.size(); ++i) {
        float t0 = INFINITY, t1 = INFINITY;
        if (spheres[i].intersect(rayorig, raydir, t0, t1)) {
            if (t0 < 0) t0 = t1;
            if (t0 < tnear) {
                tnear = t0;
                sphere = &spheres[i];
            }
        }
    }
    // if there's no intersection return black or background color
    if (!sphere) return Vec3f(2);
    Vec3f surfaceColor = 0; // color of the ray/surfaceof the object intersected by the ray
    Vec3f phit = rayorig + raydir * tnear; // point of intersection
    Vec3f nhit = phit - sphere->center; // normal at the intersection point
    nhit.normalize(); // normalize normal direction
    // If the normal and the view direction are not opposite to each other
    // reverse the normal direction. That also means we are inside the sphere so set
    // the inside bool to true. Finally reverse the sign of IdotN which we want
    // positive.
    float bias = 1e-4; // add some bias to the point from which we will be tracing
    bool inside = false;
    if (raydir.dot(nhit) > 0) nhit = -nhit, inside = true;
    if ((sphere->transparency > 0 || sphere->reflection > 0) && depth < MAX_RAY_DEPTH) {
        float facingratio = -raydir.dot(nhit);
        // change the mix value to tweak the effect
        float fresneleffect = mix(pow(1 - facingratio, 3), 1, 0.1);
        // compute reflection direction (not need to normalize because all vectors
        // are already normalized)
        Vec3f refldir = raydir - nhit * 2 * raydir.dot(nhit);
        refldir.normalize();
        Vec3f reflection = trace(phit + nhit * bias, refldir, spheres, depth + 1);
        //Vec3f reflection = trace(phit, refldir, spheres, depth + 1); little change in the final effect
        Vec3f refraction = 0;
        // if the sphere is also transparent compute refraction ray (transmission)
        if (sphere->transparency) {
            float ior = 1.1, eta = (inside) ? ior : 1 / ior; // are we inside or outside the surface?
            float cosi = -nhit.dot(raydir);
            float k = 1 - eta * eta * (1 - cosi * cosi);
            Vec3f refrdir = raydir * eta + nhit * (eta *  cosi - sqrt(k));
            refrdir.normalize();
            refraction = trace(phit - nhit * bias, refrdir, spheres, depth + 1);
        }
        // the result is a mix of reflection and refraction (if the sphere is transparent)
        surfaceColor = (
                           reflection * fresneleffect +
                           refraction * (1 - fresneleffect) * sphere->transparency) * sphere->surfaceColor;
    }
    else {
        // it's a diffuse object, no need to raytrace any further
        for (unsigned i = 0; i < spheres.size(); ++i) {
            if (spheres[i].emissionColor.x > 0) {
                // this is a light
                Vec3f transmission = 1;
                Vec3f lightDirection = spheres[i].center - phit;
                lightDirection.normalize();
                for (unsigned j = 0; j < spheres.size(); ++j) {
                    if (i != j) {
                        float t0, t1;
                        if (spheres[j].intersect(phit + nhit * bias, lightDirection, t0, t1)) {
                            transmission = 0;
                            break;
                        }
                    }
                }
                surfaceColor += sphere->surfaceColor * transmission *
                                std::max(float(0), nhit.dot(lightDirection)) * spheres[i].emissionColor;
            }
        }
    }

    return surfaceColor + sphere->emissionColor;
}

static void process8uC3( BackgroundSubtractorMOG& obj, const Mat& image, Mat& fgmask, double learningRate )
{
    int x, y, k, k1, rows = image.rows, cols = image.cols;
    float alpha = (float)learningRate, T = (float)obj.backgroundRatio, vT = (float)obj.varThreshold;
    int K = obj.nmixtures;
    
    const float w0 = (float)CV_BGFG_MOG_WEIGHT_INIT;
    const float sk0 = (float)(w0/CV_BGFG_MOG_SIGMA_INIT*sqrt(3.));
    const float var0 = (float)(CV_BGFG_MOG_SIGMA_INIT*CV_BGFG_MOG_SIGMA_INIT);
    const float minVar = (float)(obj.noiseSigma*obj.noiseSigma);
    MixData<Vec3f>* mptr = (MixData<Vec3f>*)obj.bgmodel.data;
    
    for( y = 0; y < rows; y++ )
    {
        const uchar* src = image.ptr<uchar>(y);
        uchar* dst = fgmask.ptr<uchar>(y);
        
        if( alpha > 0 )
        {
            for( x = 0; x < cols; x++, mptr += K )
            {
                float wsum = 0;
                Vec3f pix(src[x*3], src[x*3+1], src[x*3+2]);
                int kHit = -1, kForeground = -1;
                
                for( k = 0; k < K; k++ )
                {
                    float w = mptr[k].weight;
                    wsum += w;
                    if( w < FLT_EPSILON )
                        break;
                    Vec3f mu = mptr[k].mean;
                    Vec3f var = mptr[k].var;
                    Vec3f diff = pix - mu;
                    float d2 = diff.dot(diff);
                    if( d2 < vT*(var[0] + var[1] + var[2]) )
                    {
                        wsum -= w;
                        float dw = alpha*(1.f - w);
                        mptr[k].weight = w + dw;
                        mptr[k].mean = mu + alpha*diff;
                        var = Vec3f(max(var[0] + alpha*(diff[0]*diff[0] - var[0]), minVar),
                                    max(var[1] + alpha*(diff[1]*diff[1] - var[1]), minVar),
                                    max(var[2] + alpha*(diff[2]*diff[2] - var[2]), minVar));
                        mptr[k].var = var;
                        mptr[k].sortKey = w/sqrt(var[0] + var[1] + var[2]);
                        
                        for( k1 = k-1; k1 >= 0; k1-- )
                        {
                            if( mptr[k1].sortKey >= mptr[k1+1].sortKey )
                                break;
                            std::swap( mptr[k1], mptr[k1+1] );
                        }
                        
                        kHit = k1+1;
                        break;
                    }
                }
                
                if( kHit < 0 ) // no appropriate gaussian mixture found at all, remove the weakest mixture and create a new one
                {
                    kHit = k = min(k, K-1);
                    wsum += w0 - mptr[k].weight;
                    mptr[k].weight = w0;
                    mptr[k].mean = pix;
                    mptr[k].var = Vec3f(var0, var0, var0);
                    mptr[k].sortKey = sk0;
                }
                else
                    for( ; k < K; k++ )
                        wsum += mptr[k].weight;
            
                float wscale = 1.f/wsum;
                wsum = 0;
                for( k = 0; k < K; k++ )
                {
                    wsum += mptr[k].weight *= wscale;
                    mptr[k].sortKey *= wscale;
                    if( wsum > T && kForeground < 0 )
                        kForeground = k+1;
                }
                
                dst[x] = (uchar)(-(kHit >= kForeground));
            }
        }
        else
        {
            for( x = 0; x < cols; x++, mptr += K )
            {
                Vec3f pix(src[x*3], src[x*3+1], src[x*3+2]);
                int kHit = -1, kForeground = -1;
                
                for( k = 0; k < K; k++ )
                {
                    if( mptr[k].weight < FLT_EPSILON )
                        break;
                    Vec3f mu = mptr[k].mean;
                    Vec3f var = mptr[k].var;
                    Vec3f diff = pix - mu;
                    float d2 = diff.dot(diff);
//.........这里部分代码省略.........

void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
                                 Vec3f& VEC, Vec3f& X, Vec3f& Y)
{
  Vec3f T;
  FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
  Vec3f TMP;

  T = Q - P;
  A_dot_A = A.dot(A);
  B_dot_B = B.dot(B);
  A_dot_B = A.dot(B);
  A_dot_T = A.dot(T);
  B_dot_T = B.dot(T);

  // t parameterizes ray P,A
  // u parameterizes ray Q,B

  FCL_REAL t, u;

  // compute t for the closest point on ray P,A to
  // ray Q,B

  FCL_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;

  t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;

  // clamp result so t is on the segment P,A

  if((t < 0) || boost::math::isnan(t)) t = 0; else if(t > 1) t = 1;

  // find u for point on ray Q,B closest to point at t

  u = (t*A_dot_B - B_dot_T) / B_dot_B;

  // if u is on segment Q,B, t and u correspond to
  // closest points, otherwise, clamp u, recompute and
  // clamp t

  if((u <= 0) || boost::math::isnan(u))
  {
    Y = Q;

    t = A_dot_T / A_dot_A;

    if((t <= 0) || boost::math::isnan(t))
    {
      X = P;
      VEC = Q - P;
    }
    else if(t >= 1)
    {
      X = P + A;
      VEC = Q - X;
    }
    else
    {
      X = P + A * t;
      TMP = T.cross(A);
      VEC = A.cross(TMP);
    }
  }
  else if (u >= 1)
  {
    Y = Q + B;

    t = (A_dot_B + A_dot_T) / A_dot_A;

    if((t <= 0) || boost::math::isnan(t))
    {
      X = P;
      VEC = Y - P;
    }
    else if(t >= 1)
    {
      X = P + A;
      VEC = Y - X;
    }
    else
    {
      X = P + A * t;
      T = Y - P;
      TMP = T.cross(A);
      VEC= A.cross(TMP);
    }
  }
  else
  {
    Y = Q + B * u;

    if((t <= 0) || boost::math::isnan(t))
    {
      X = P;
      TMP = T.cross(B);
      VEC = B.cross(TMP);
    }
    else if(t >= 1)
    {
      X = P + A;
      T = Q - X;
      TMP = T.cross(B);
//.........这里部分代码省略.........

void Mesher::check_feature()
{
    auto contour = get_contour();
    const auto normals = get_normals(contour);

    // Find the largest cone and the normals that enclose
    // the largest angle as n0, n1.
    float theta = 1;
    Vec3f n0, n1;
    for (auto ni : normals)
    {
        for (auto nj : normals)
        {
            float dot = ni.dot(nj);
            if (dot < theta)
            {
                theta = dot;
                n0 = ni;
                n1 = nj;
            }
        }
    }

    // If there isn't a feature in this fan, then return immediately.
    if (theta > 0.9)
        return;

    // Decide whether this is a corner or edge feature.
    const Vec3f nstar = n0.cross(n1);
    float phi = 0;
    for (auto n : normals)
        phi = fmax(phi, fabs(nstar.dot(n)));
    bool edge = phi < 0.7;

    // Find the center of the contour.
    Vec3f center(0, 0, 0);
    for (auto c : contour)
        center += c;
    center /= contour.size();

    // Construct the matrices for use in our least-square fit.
    Eigen::MatrixX3d A(normals.size(), 3);
    {
        int i=0;
        for (auto n : normals)
            A.row(i++) << n.transpose();
    }

    // When building the second matrix, shift position values to be centered
    // about the origin (because that's what the least-squares fit will
    // minimize).
    Eigen::VectorXd B(normals.size(), 1);
    {
        auto n = normals.begin();
        auto c = contour.begin();
        int i=0;
        while (n != normals.end())
            B.row(i++) << (n++)->dot(*(c++) - center);
    }

    // Use singular value decomposition to solve the least-squares fit.
    Eigen::JacobiSVD<Eigen::MatrixX3d> svd(A, Eigen::ComputeFullU |
                                              Eigen::ComputeFullV);

    // Set the smallest singular value to zero to make fitting happier.
    if (edge)
    {
        auto singular = svd.singularValues();
        svd.setThreshold(singular.minCoeff() / singular.maxCoeff() * 1.01);
    }

    // Solve for the new point's position.
    const Vec3f new_pt = svd.solve(B) + center;

    // Erase this triangle fan, as we'll be inserting a vertex in the center.
    triangles.erase(fan_start, voxel_start);

    // Construct a new triangle fan.
    contour.push_back(contour.front());
    {
        auto p0 = contour.begin();
        auto p1 = contour.begin();
        p1++;
        while (p1 != contour.end())
            push_swappable_triangle(Triangle(*(p0++), *(p1++), new_pt));
    }
}

float Plane::calcDistance(const Vec3f p) const
{
	return p.dot(normal) - distance;
}

Vec3f proj (Vec3f v1, Vec3f v2){
		//projection v1 to v2:
	// a1 = (a dot b)/(b dot b)   * b 
	return (((v1.dot(v2))/(v2.dot(v2)))* v2);
}

int PointTracker::POSIT(float f)
{
	// POSIT algorithm for coplanar points as presented in
	// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
	// we use the same notation as in the paper here

	// The expected rotation used for resolving the ambiguity in POSIT:
	// In every iteration step the rotation closer to R_expected is taken 
	Matx33f R_expected;	
	if (init_phase)
		R_expected = Matx33f::eye(); // in the init phase, we want to be close to the default pose = no rotation
	else 
		R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation
	
	// initial pose = last (predicted) pose
	Vec3f k;
	get_row(X_CM.R, 2, k);
	float Z0 = X_CM.t[2];

	float old_epsilon_1 = 0;
	float old_epsilon_2 = 0;
	float epsilon_1 = 1;
	float epsilon_2 = 1;

	Vec3f I0, J0;
	Vec2f I0_coeff, J0_coeff;

	Vec3f I_1, J_1, I_2, J_2;
	Matx33f R_1, R_2;
	Matx33f* R_current;

	const int MAX_ITER = 100;
	const float EPS_THRESHOLD = 1e-4;

	int i=1;
	for (; i<MAX_ITER; ++i)
	{
		epsilon_1 = k.dot(point_model->M01)/Z0;
		epsilon_2 = k.dot(point_model->M02)/Z0;

		// vector of scalar products <I0, M0i> and <J0, M0i>
		Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0], 
			         p[2][0]*(1.0 + epsilon_2) - p[0][0]);
		Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1],
			         p[2][1]*(1.0 + epsilon_2) - p[0][1]);

		// construct projection of I, J onto M0i plane: I0 and J0
		I0_coeff = point_model->P * I0_M0i;
		J0_coeff = point_model->P * J0_M0i;
		I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02;
		J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02;

		// calculate u component of I, J		
		float II0 = I0.dot(I0);
		float IJ0 = I0.dot(J0);
		float JJ0 = J0.dot(J0);
		float rho, theta;
		if (JJ0 == II0) {
			rho = sqrt(abs(2*IJ0));
			theta = -PI/4;
			if (IJ0<0) theta *= -1;
		}
		else {
			rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 ));
			theta = atan( -2*IJ0 / (JJ0-II0) );
			if (JJ0 - II0 < 0) theta += PI;
			theta /= 2;
		}

		// construct the two solutions
		I_1 = I0 + rho*cos(theta)*point_model->u;	
		I_2 = I0 - rho*cos(theta)*point_model->u;

		J_1 = J0 + rho*sin(theta)*point_model->u;
		J_2 = J0 - rho*sin(theta)*point_model->u;

		float norm_const = 1.0/norm(I_1); // all have the same norm
		
		// create rotation matrices
		I_1 *= norm_const; J_1 *= norm_const;
		I_2 *= norm_const; J_2 *= norm_const;

		set_row(R_1, 0, I_1);
		set_row(R_1, 1, J_1);
		set_row(R_1, 2, I_1.cross(J_1));
		
		set_row(R_2, 0, I_2);
		set_row(R_2, 1, J_2);
		set_row(R_2, 2, I_2.cross(J_2));

		// the single translation solution
		Z0 = norm_const * f;

		// pick the rotation solution closer to the expected one
		// in simple metric d(A,B) = || I - A * B^T ||
		float R_1_deviation = norm(Matx33f::eye() - R_expected * R_1.t());
		float R_2_deviation = norm(Matx33f::eye() - R_expected * R_2.t());

		if (R_1_deviation < R_2_deviation)
			R_current = &R_1;
//.........这里部分代码省略.........

Vec3f PhotonTracer::traceSample(Vec2u pixel, const KdTree<Photon> &surfaceTree,
        const KdTree<VolumePhoton> *mediumTree, PathSampleGenerator &sampler,
        float gatherRadius)
{
    PositionSample point;
    if (!_scene->cam().samplePosition(sampler, point))
        return Vec3f(0.0f);
    DirectionSample direction;
    if (!_scene->cam().sampleDirection(sampler, point, pixel, direction))
        return Vec3f(0.0f);
    sampler.advancePath();

    Vec3f throughput = point.weight*direction.weight;
    Ray ray(point.p, direction.d);
    ray.setPrimaryRay(true);

    IntersectionTemporary data;
    IntersectionInfo info;
    const Medium *medium = _scene->cam().medium().get();

    Vec3f result(0.0f);
    int bounce = 0;
    bool didHit = _scene->intersect(ray, data, info);
    while ((medium || didHit) && bounce < _settings.maxBounces - 1) {
        if (medium) {
            if (mediumTree) {
                Vec3f beamEstimate(0.0f);
                mediumTree->beamQuery(ray.pos(), ray.dir(), ray.farT(), [&](const VolumePhoton &p, float t, float distSq) {
                    Ray mediumQuery(ray);
                    mediumQuery.setFarT(t);
                    beamEstimate += (3.0f*INV_PI*sqr(1.0f - distSq/p.radiusSq))/p.radiusSq
                            *medium->phaseFunction(p.pos)->eval(ray.dir(), -p.dir)
                            *medium->transmittance(mediumQuery)*p.power;
                });
                result += throughput*beamEstimate;
            }
            throughput *= medium->transmittance(ray);
        }
        if (!didHit)
            break;

        const Bsdf &bsdf = *info.bsdf;

        SurfaceScatterEvent event = makeLocalScatterEvent(data, info, ray, &sampler);

        Vec3f transparency = bsdf.eval(event.makeForwardEvent(), false);
        float transparencyScalar = transparency.avg();

        Vec3f wo;
        if (sampler.nextBoolean(DiscreteTransparencySample, transparencyScalar)) {
            wo = ray.dir();
            throughput *= transparency/transparencyScalar;
        } else {
            event.requestedLobe = BsdfLobes::SpecularLobe;
            if (!bsdf.sample(event, false))
                break;

            wo = event.frame.toGlobal(event.wo);

            throughput *= event.weight;
        }

        bool geometricBackside = (wo.dot(info.Ng) < 0.0f);
        medium = info.primitive->selectMedium(medium, geometricBackside);

        ray = ray.scatter(ray.hitpoint(), wo, info.epsilon);

        if (std::isnan(ray.dir().sum() + ray.pos().sum()))
            break;
        if (std::isnan(throughput.sum()))
            break;

        sampler.advancePath();
        bounce++;
        if (bounce < _settings.maxBounces)
            didHit = _scene->intersect(ray, data, info);
    }

    if (!didHit) {
        if (!medium && _scene->intersectInfinites(ray, data, info))
            result += throughput*info.primitive->evalDirect(data, info);
        return result;
    }
    if (info.primitive->isEmissive())
        result += throughput*info.primitive->evalDirect(data, info);

    int count = surfaceTree.nearestNeighbours(ray.hitpoint(), _photonQuery.get(), _distanceQuery.get(),
            _settings.gatherCount, gatherRadius);
    if (count == 0)
        return result;

    const Bsdf &bsdf = *info.bsdf;
    SurfaceScatterEvent event = makeLocalScatterEvent(data, info, ray, &sampler);

    Vec3f surfaceEstimate(0.0f);
    for (int i = 0; i < count; ++i) {
        event.wo = event.frame.toLocal(-_photonQuery[i]->dir);
        // Asymmetry due to shading normals already compensated for when storing the photon,
        // so we don't use the adjoint BSDF here
        surfaceEstimate += _photonQuery[i]->power*bsdf.eval(event, false)/std::abs(event.wo.z());
//.........这里部分代码省略.........

void DVRClipGeometry::linkContour(      DVRTriangle *startTriangle,
                                        Real32       dist2RefPlane,
                                  const Vec3f       &viewDir, 
                                        bool         positiveWinding)
{
    FDEBUG(("DVRClipGeometry - linkcontour dist = %f\n", dist2RefPlane));
  
    bool closed = false;

    // first, we have to check for the correct winding direction.

    Pnt3f vertex[2];
    bool firstEdge;
    int  first = 0, second = 0;

    if(startTriangle->edgeCut[0] && startTriangle->edgeCut[1])
    {
        vertex[0] = interpolate(startTriangle, 1, 0, dist2RefPlane); 
        vertex[1] = interpolate(startTriangle, 1, 2, dist2RefPlane);

        first = 0; second = 1;
    }
    else if (startTriangle->edgeCut[1] && startTriangle->edgeCut[2])
    {
        vertex[0] = interpolate(startTriangle, 2, 1, dist2RefPlane);
        vertex[1] = interpolate(startTriangle, 2, 0, dist2RefPlane);

        first = 1; second = 2;
    }
    else if (startTriangle->edgeCut[0] && startTriangle->edgeCut[2])
    {
        vertex[0] = interpolate(startTriangle, 0, 1, dist2RefPlane);
        vertex[1] = interpolate(startTriangle, 0, 2, dist2RefPlane);

        first = 0; second = 2;
    }

    // Now we should have both cut points on our edges.

    // If the cross product of the normal of this triangle with the 
    // vector between the two cut points (cutPoint[1] - cutPoint[0]) 
    // has a positive dot product with the viewing direction, then 
    // the edge with cutPoint[0] on it is the right direction, otherwise
    // we would have to choose the other direction.

    Vec3f tmp = vertex[1] - vertex[0];

    tmp = tmp.cross(startTriangle->transformedNormal);

    if(tmp.dot(viewDir) <= 0.0)
    {
        firstEdge = false;
    }else
    {
        firstEdge = true;
    }

    if(!positiveWinding)
        firstEdge = !firstEdge;

    DVRTriangle *current = startTriangle;

    current->inContour = true;

    if(firstEdge)
    {
        current->cutPnt      = vertex[0];
        current->cutPoint[0] = vertex[0][0];
        current->cutPoint[1] = vertex[0][1];
        current->cutPoint[2] = vertex[0][2];

        current->contourNeighbour = &_mfTriangles[current->neighbours[first]];

        //      // debugging -> remove
        //      if(!current->contourNeighbour){
        //        std::cerr<<"contour neighbour is NULL\n";
        //        exit(0);
        //      }

        current = current->contourNeighbour;
    }
    else
    {
        current->cutPnt      = vertex[1];
        current->cutPoint[0] = vertex[1][0];
        current->cutPoint[1] = vertex[1][1];
        current->cutPoint[2] = vertex[1][2];    

        current->contourNeighbour = &_mfTriangles[current->neighbours[second]];
        //      // debugging -> remove
        //      if(!current->contourNeighbour){
        //        std::cerr<<"contour neighbour is NULL\n";
        //        exit(0);
        //      }

        current = current->contourNeighbour;    
    }

    //check neighbours
    while(!closed)
//.........这里部分代码省略.........

  void doInteraction(float dt){
    Glove *l = &left;
    Glove *r = &right;

    Vec3f headPos = tracker->markerPositions[17]; // right(0-7) left(8-15) A B C (16 17 18)
    if(headPos.mag() == 0) return;

    Rayd rayS(headPos, r->centroid - headPos); // ray pointing in direction of head to right hand
    
    float t = rayS.intersectAllosphere(); // get t on surface of allosphere screen
    Vec3f pos = nav().quat().rotate( rayS(t) ); // rotate point on allosphere to match current nav orientation (check this)
    Rayd ray( nav().pos(), pos); // ray from sphere center (camera location) to intersected location

    // Use ray to intesect with plasma shell on pinch
    if( r->pinchOn[eIndex]){
      cout<<"right index finger pinched, create lightning!"<<endl; 
      //audio
      //cout << currentPlayer << endl;
      //cout << *currentPlayer << endl;
      samplePlayer[*currentPlayer].reset(); // reset the phase == start playing the sound

      *currentPlayer = *currentPlayer + 1;
      if (*currentPlayer == N_SAMPLE_PLAYER){
        *currentPlayer = 0;
        // currentPlayer = 0; // this was a horrible BUG! XXX
      }
      //visual
      Vec3f center = Vec3f(0, 0.6, -1);
      float t = ray.intersectSphere(Vec3f(0,0,0), R);
      Vec3f src = ray(t);
      Vec3f dest = 0.1f * (src - center).normalize() + center;
      Bolt* newPSBolt = new Bolt();
      newPSBolt->makeTexture();
      newPSBolt->start = src;
      newPSBolt->ending = dest;
      newPSBolt->makeBolt(src, dest, 2, 0.03);
      newPSBolt->createBulge(center);
      boltQ->push_back(newPSBolt); 
    }

    // state->cursor.set(nav().pos() + pos);

    // Navigation joystick mode
    // Translation done with the left index finger pinch gesture
    if( l->pinchOn[eIndex]){
    } else if( l->pinched[eIndex]){
      Vec3f translate = sensitivity * l->getPinchTranslate(eIndex);
      for(int i=0; i<3; i++){
        nav().pos()[i] = nav().pos()[i] * 0.9 + 
          (nav().pos()[i] + translate.dot(Vec3d(nav().ur()[i], nav().uu()[i], -nav().uf()[i]))) * 0.1;
      }
      // nav().pos().lerp( nav().pos() + translate, 0.01f);
    } else if( l->pinchOff[eIndex] ){
    } else {
    }
    //ePinky
    //eRing

    // change navigation sensitivity
    if( l->pinched[eMiddle]){
      Vec3f v = l->getPinchTranslate(eMiddle);
      sensitivity = abs(v.y*10);
    }

  }

BVH_REAL quadraticForm(const Vec3f M[3], const Vec3f& v)
{
  return v.dot(Vec3f(M[0].dot(v), M[1].dot(v), M[2].dot(v)));
}

bool Cylinder::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
	if(samples.size() < 4)
		return false;
	// estimate axis from covariance of normal vectors
	MiscLib::Vector< GfxTL::Vector3Df > normals;
	size_t c = samples.size() / 2;
	for(size_t i = c; i < samples.size(); ++i)
	{
		normals.push_back(GfxTL::Vector3Df(samples[i]));
		normals.push_back(GfxTL::Vector3Df(-samples[i]));
	}
	GfxTL::MatrixXX< 3, 3, float > cov, eigenVectors;
	GfxTL::Vector3Df eigenValues;
	GfxTL::CovarianceMatrix(GfxTL::Vector3Df(0, 0, 0),
		normals.begin(), normals.end(), &cov);
	GfxTL::Jacobi(cov, &eigenValues, &eigenVectors);
	// find the minimal eigenvalue and corresponding vector
	float minEigVal = eigenValues[0];
	unsigned int minEigIdx = 0;
	for(unsigned int i = 1; i < 3; ++i)
		if(eigenValues[i] < minEigVal)
		{
			minEigVal = eigenValues[i];
			minEigIdx = i;
		}
	m_axisDir = Vec3f(eigenVectors[minEigIdx]);
	// get a point on the axis from all pairs
	m_axisPos = Vec3f(0, 0, 0);
	m_radius = 0;
	size_t pointCount = 0;
	size_t pairCount = 0;
	for(size_t i = 0; i < c - 1; ++i)
		for(size_t j = i + 1; j < c; ++j)
		{
			// project first normal into plane
			float l = m_axisDir.dot(samples[i + c]);
			Vec3f xdir = samples[i + c] - l * m_axisDir;
			xdir.normalize();
			Vec3f ydir = m_axisDir.cross(xdir);
			ydir.normalize();
			// xdir is the x axis in the plane (y = 0) samples[i] is the origin
			float lineBnx = ydir.dot(samples[j + c]);
			if(abs(lineBnx) < .05f)
				continue;
			float lineBny = -xdir.dot(samples[j + c]);
			// origin of lineB
			Vec3f originB = samples[j] - samples[i];
			float lineBOx = xdir.dot(originB);
			float lineBOy = ydir.dot(originB);
			float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
			// lineB in the plane complete
			// point of intersection is y = 0 and x = lineBd / lineBnx
			float radius = lineBd / lineBnx;
			m_axisPos += samples[i] + radius * xdir;
			m_radius += abs(radius);
			m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
			++pointCount;
		}
	if(!pointCount)
		return false;
	m_axisPos /= pointCount;
	m_radius /= pointCount * 2;
	if(m_radius > 1e6)
		return false;

	// find point on axis closest to origin
	float lambda = m_axisDir.dot(-m_axisPos);
	m_axisPos = m_axisPos + lambda * m_axisDir;

	m_hcs.FromNormal(m_axisDir);
	m_angularRotatedRadians = 0;
	return true;
}

 /** The computed distance is perfect in that case
  * @param p_j the point to compute its distance to
  * @return
  */
 float distance(const Vec3f& p_j) const
 {
   return std::abs(float(p_j.dot(n_) + d_));
 }

//.........这里部分代码省略.........
            newcalcd = sqrt(newcollision.x * newcollision.x * COLLIDESIZERATIO + newcollision.y * newcollision.y * COLLIDESIZERATIO + newcollision.z * newcollision.z / COLLIDESIZERATIO);
            //		printf( "%f-%f %f-%f %f-%f ---\n", newcollisionx,newcollisiony,newcollisionz );

            if (newcalcd > COLLIDESIZE) continue;
            float press = (COLLIDESIZE - newcalcd) / COLLIDESIZE;

            tp->id = i;
            gamemap.collision(tp);

            /*
                            printf( " +%f (%f %f %f)  ->\n", press, newcollisionx, newcollisiony, newcollisionz );
                            printf( "  %f %f %f %f\n", tp->Position.r, tp->Position.g, tp->Position.b, tp->Position.a );   //x,y,z,unused
                            printf( "  %f %f %f %f\n", tp->Direction.r, tp->Direction.g, tp->Direction.b, tp->Direction.a );   //x,y,z,unused
                            printf( "  %f %f %f %f\n", tp->AuxRotation.r, tp->AuxRotation.g, tp->AuxRotation.b, tp->AuxRotation.a );   //x,y,z,unused
                            printf( "  %f %f %f %f\n", tp->NewDirection.r, tp->NewDirection.g, tp->NewDirection.b, tp->NewDirection.a );   //x,y,z,unused
                            printf( "  %f %f %f %f\n", tp->Normal.r, tp->Normal.g, tp->Normal.b, tp->Normal.a );   //x,y,z,unused
                            printf( "  %f %f %f %f\n", tp->InAreaWarp.r, tp->InAreaWarp.g, tp->InAreaWarp.b, tp->InAreaWarp.a );   //x,y,z,unused
                            printf( "  %f %f %f %f\n", tp->TargetLocation.r, tp->TargetLocation.g, tp->TargetLocation.b, tp->TargetLocation.a );   //x,y,z,unused
             */

            //If it is a bottom probe, we are on the ground.
            if (i > int(probes.size()) - 3) {
                gTimeSinceOnGround = 0;
                //gh.vZ = 0;
            }


            //First of all, nerf any motion toward the collision.
            {
                Vec3f ns = gh.v;
                ns /= ns.len();


                Vec3f dot {nd.x*ns.x,nd.y*ns.y,nd.z*ns.z};
                if (dot.x > 0) {
                    gh.v.x *= (1. - dot.x * press) * VCFM;
                }
                if (dot.y > 0) {
                    gh.v.y *= (1. - dot.y * press) * VCFM;
                }
                if (dot.z > 0) {
                    gh.v.z *= (1. - dot.z * press * VCFM);
                }
            }
            //Next, push the MapOffset back
            //(Change newx, newy, newz)

            //(ndx,ndy,ndz) represents ray.
            //(tx, ty, tz)  represents target ray hit.
            //press = distance of compression.

            Vec3f nid = newcollision * press;
            nid = nid * CFM;
            nid = {nid.x*iaw.x,nid.y*iaw.y,nid.z*iaw.z};

            if (sqrt(nid.x * nid.x + nid.y * nid.y) < MINSIDE) {
                nid.x = nid.y = 0;
            }

            newp.x -= nid.x * fabs(sfn.x);
            newp.y -= nid.y * fabs(sfn.y);
            newp.z -= nid.z * fabs(sfn.z);
        }

    if (!gGodMode) {
        gh.MapOffset = newp;

//return a convex hull connectivity from
//points .... array of points
//count .... size of array
//l .... line vector(T1-T2)
//ln .... line normal
//prallel lines on a Z=const plane are parallel under perspective projections -> similar
//Projecting points to a single Z plane
//If we always project to a fixed plane than koeficients can be fixed
void Scene::flatten(Vec3f *points, int count, Vec3f l, Vec3f ln, ConvexHull &ch) {
	//TODO : For now we are using a simplified method of returning a bounding rectangle
	//Maybe good enough, but it should return a convex hull
	//NOT GOOD

	//FIND max line length and max distance between lines
	// distance between lines by finding min and max signed distance between line point and first point projected to "ln"

	Vec3f T1, T2, T3, T4;
	//ConvexH c; - bullet versionn

	int idmin = 0, idmax = 0;
	float lmax = 0, len;
	float d[12];
	points[0].x = points[0].x * m_fZNear / points[0].z;
	points[0].y = points[0].y * m_fZNear / points[0].z;
	points[0].z = m_fZNear;
	for (int i = 0; i < count / 2; i++) {
		//Projecting points to m_fZNear
		points[2 * i].x = points[2 * i].x * m_fZNear / points[2 * i].z;
		points[2 * i].y = points[2 * i].y * m_fZNear / points[2 * i].z;
		points[2 * i].z = m_fZNear;
		points[2 * i + 1].x = points[2 * i + 1].x * m_fZNear / points[2 * i + 1].z;
		points[2 * i + 1].y = points[2 * i + 1].y * m_fZNear / points[2 * i + 1].z;
		points[2 * i + 1].z = m_fZNear;

		//gathering span
		d[i] = ln.dot(points[2 * i] - points[0]);

		len = Vec3f(points[2 * i] - points[2 * i + 1]).length();
		idmin = d[i] < d[idmin] ? i : idmin;
		idmax = d[i] > d[idmax] ? i : idmax;
		lmax = len > lmax ? len : lmax;
	}

	//we need to sort points starting from dmin going to dmax
	float yzk[12];
	float* indArr[12];
	for (int i = 0; i < count / 2; i++) {
		yzk[i] = d[i]; // / ch.points[i].z; //They are already projected
		indArr[i] = &yzk[i];
	}

	//Sorting by span
	qsort(indArr, count / 2, sizeof(float*), compre_floats);

	//Generating points and connectivity information for convex polygon
	int ch_pc = 0;
	int point_idxs[6];
	point_idxs[0] = 2 * (indArr[0] - yzk);

	for (int i = 0; i < count / 2 - 1;) {
		int temp = i + 1;
		int ith = indArr[i] - yzk;
		int ihtplus1 = indArr[i + 1] - yzk;
		Vec3f pi = points[2 * ith];
		Vec3f pj = points[2 * ihtplus1];
		float dy = (pj.y - pi.y);
		float dx = (pj.x - pi.x);
		float k = dx != 0 ? dy / dx : 100000.0;
		for (int j = i + 2; j < count / 2; ++j) {
			int jth = indArr[j] - yzk;
			pj = points[2 * jth];
			dy = (pj.y - pi.y);
			dx = (pj.x - pi.x);
			float kj = dx != 0 ? dy / dx : 100000.0;
			if (((kj < k) && kj < 0) || ((kj > k) && kj > 0)) {
				k = kj;
				temp = j;
			}
		}
		i = temp;
		ch_pc++;
		point_idxs[ch_pc] = 2 * (indArr[i] - yzk);
	}

	ch_pc++;
	point_idxs[ch_pc] = 2 * (indArr[count / 2] - yzk);

	ch.points[0] = points[point_idxs[0]];
	ch.points[1] = points[point_idxs[0] + 1];
	ch.connectivity[0] = 1;
	ch.connectivity[1] = 2;
	ch.connectivity[2] = 0;
	ch.connectivity[3] = 3;

	int var;
	for (var = 1; var < ch_pc - 1; var++) {
		ch.points[2 * var] = points[point_idxs[var]];
		ch.connectivity[2 * (2 * var)] = 2 * var - 2;
		ch.connectivity[2 * (2 * var) + 1] = 2 * var + 2;

//.........这里部分代码省略.........