C++ Vec3d::cross方法代码示例

cv::Affine3d cv::viz::makeCameraPose(const Vec3d& position, const Vec3d& focal_point, const Vec3d& y_dir)
{
    // Compute the transformation matrix for drawing the camera frame in a scene
    Vec3d n = normalize(focal_point - position);
    Vec3d u = normalize(y_dir.cross(n));
    Vec3d v = n.cross(u);

    return makeTransformToGlobal(u, v, n, position);
}

void Viz::issueNormal(vector<GLfloat> &normal, vector<GLfloat> &vertices, vector<GLuint> &indices, double mult) {
  normal.clear();
  int mxId = 0;
  for (int i = 0; i < indices.size(); i++) 
    if (indices[i] > mxId)
      mxId = indices[i];

  vector<Vec3d> norms(mxId + 1);
  vector<int> count(mxId + 1);
  for (int i = 0; i < indices.size(); i+= 3) {
    unsigned int *p = &indices[i];
    Vec3d a(vertices[p[0] * 3], vertices[p[0] * 3 + 1], vertices[p[0] * 3 + 2]);
    Vec3d b(vertices[p[1] * 3], vertices[p[1] * 3 + 1], vertices[p[1] * 3 + 2]);
    Vec3d c(vertices[p[2] * 3], vertices[p[2] * 3 + 1], vertices[p[2] * 3 + 2]);
    Vec3d ab = b - a;
    Vec3d ac = c - a;
    Vec3d cp = ab.cross(ac);
    cp /= norm(cp);

    for (int j = 0; j < 3; j++) {
      //count[p[j]]++;
      //slerp(norms[p[j]], cp, norms[p[j]], 1.0 / count[p[j]]);
      norms[p[j]] += cp;
      count[p[j]] ++;
    }
  }
  for (int i = 0; i < norms.size(); i++) {
    norms[i] /= norm(norms[i]);
    norms[i] *= mult;
    normal.push_back(norms[i][0]);
    normal.push_back(norms[i][1]);
    normal.push_back(norms[i][2]);
  }
}

// Kok Lim Low's linearization
static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Vec3d& rpy, Vec3d& t)
{
  //Mat sub = Dst - Src;
  Mat A = Mat(Src.rows, 6, CV_64F);
  Mat b = Mat(Src.rows, 1, CV_64F);
  Mat rpy_t;

#if defined _OPENMP
#pragma omp parallel for
#endif
  for (int i=0; i<Src.rows; i++)
  {
    const Vec3d srcPt(Src.ptr<double>(i));
    const Vec3d dstPt(Dst.ptr<double>(i));
    const Vec3d normals(Dst.ptr<double>(i) + 3);
    const Vec3d sub = dstPt - srcPt;
    const Vec3d axis = srcPt.cross(normals);

    *b.ptr<double>(i) = sub.dot(normals);
    hconcat(axis.reshape<1, 3>(), normals.reshape<1, 3>(), A.row(i));
  }

  cv::solve(A, b, rpy_t, DECOMP_SVD);
  rpy_t.rowRange(0, 3).copyTo(rpy);
  rpy_t.rowRange(3, 6).copyTo(t);
}

void Camera::getBillboardMatrix(float modelview[16]) {
	Vec3d right = direction.cross(up).unit();
	for (int i = 0; i < 3; i++) {
		Vec3d v;
		switch(i) {
			case 0:
				v = right;
				break;
			case 1:
				v = right.cross(direction).unit();
				break;
			case 2:
				v = direction.unit();
				break;
			case 3:
				v = ZERO_VEC3d;
				break;
		}
		modelview[i*4]     = v.x;
		modelview[i*4 + 1] = v.y;
		modelview[i*4 + 2] = v.z;
		modelview[i*4 + 3] = 0;
	}
	modelview[15] = 1;
}

void MassPointSystem::buildDoubleTetrahedron(const Vec3d &center, const Vec3d &vel, const Vec3d &angVel)
{
    massPointVector.push_back(MassPoint(center + Vec3d(0.0, 0.0, 4.082), vel + angVel.cross(Vec3d(0.0, 0.0, 4.082))));
    massPointVector.push_back(MassPoint(center + Vec3d(-1.443, -2.5, 0.0), vel + angVel.cross(Vec3d(-1.443, -2.5, 0.0))));
    massPointVector.push_back(MassPoint(center + Vec3d(-1.443, 2.5, 0.0), vel + angVel.cross(Vec3d(-1.443, 2.5, 0.0))));
    massPointVector.push_back(MassPoint(center + Vec3d(2.886, 0.0, 0.0), vel + angVel.cross(Vec3d(2.886, 0.0, 0.0))));
    massPointVector.push_back(MassPoint(center + Vec3d(0.0, 0.0, -4.082), vel + angVel.cross(Vec3d(0.0, 0.0, -4.082))));

    jointVector.push_back(new SpringDamperJoint(0, 1, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(0, 2, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(0, 3, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(1, 2, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(1, 3, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(1, 4, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(2, 3, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(2, 4, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(3, 4, &massPointVector));
    for (unsigned int i = 0; i < massPointVector.size(); ++i)
    {
        jointVector.push_back(new GroundContactJoint(i));
        jointVector.push_back(new GravityJoint(i));
    }
}

void glutMotion(int x, int y)
{
    if (gIsRotatingCamera)
    {
        static const double kTrackBallRadius = 0.8;   

        Vec3d lastPos;
        lastPos[0] = gLastMouseX * 2.0 / gWindowWidth - 1.0;
        lastPos[1] = (gWindowHeight - gLastMouseY) * 2.0 / gWindowHeight - 1.0;
        lastPos[2] = projectToTrackball(kTrackBallRadius, lastPos[0], lastPos[1]);

        Vec3d currPos;
        currPos[0] = x * 2.0 / gWindowWidth - 1.0;
        currPos[1] = (gWindowHeight - y) * 2.0 / gWindowHeight - 1.0;
        currPos[2] = projectToTrackball(kTrackBallRadius, currPos[0], currPos[1]);

        currPos.normalize();
        lastPos.normalize();

        Vec3d rotateVec = lastPos.cross(currPos);
				        
        double rotateAngle = asin(rotateVec.norm());
        if (fabs(rotateAngle) > 1e-6)
        {
			double deltaRotation[16];
            
			generateRotationMatrix(deltaRotation, rotateAngle, rotateVec[0], rotateVec[1], rotateVec[2]);
                
            multRight(gCameraRotation, deltaRotation);
        
            updateCamera();
        }
    }
    else if (gIsScalingCamera)
    {
        float y1 = gWindowHeight - gLastMouseY;
        float y2 = gWindowHeight - y;

        gCameraScale *= 1 + (y1 - y2) / gWindowHeight;  

        updateCamera();
    }

    gLastMouseX = x;
    gLastMouseY = y;
}

void Delaunay::computeDelaunay(Mesh &mesh)
{
	int size = (int)mesh.getVerticesSize();
	if (size == 0)
	{
		return;
	}
	mesh.computeVerticesNormals();
	m_preSize = mesh.m_curIndex;

	TriangleSet triSet;
	// 依次遍历每个点，寻找最近邻，进行三角化
	for (; mesh.m_curIndex < size; mesh.m_curIndex++)
	{
		Vertex v = mesh.getVertex(mesh.m_curIndex);
		if (v.m_isInner)
		{
			mesh.pushTriBeginIndex((int)triSet.size());
			continue;
		}

		Vec3d normal = v.m_normal;
		int id = 2;
		// 判断法向量哪个不为0，z->y->x
		if (normal[2] != 0)		// z
		{
			id = 2;
		}
		else if (normal[1] != 0)// y
		{
			id = 1;
		}
		else if (normal[0] != 0)// x
		{
			id = 0;
		}
		else	// 法向量为(0, 0, 0)，
		{
			mesh.pushTriBeginIndex((int)triSet.size());
			continue;
		}

		double minDistance = -1;
		int cnt = v.m_neighbors[0];					// 最近邻数目
		double dists[k];
		for (int j = 1; j < cnt + 1; j++)
		{
			Vec3d dv = mesh.getVertex(v.m_neighbors[j]).m_xyz - v.m_xyz;
			dists[j] = dv.ddot(dv);
		}
		minDistance = dists[1];
		VertexVector vVector, tmpvVector;
		// 将最近邻点投射到该点的切平面上
		for (int j = 1; j < cnt + 1; j++)
		{
			Vertex tmpv = mesh.getVertex(v.m_neighbors[j]);
			if (dists[j] < u * minDistance ||		// 去除非常接近的点
				(tmpv.m_index < v.m_index && tmpv.m_index >= m_preSize) ||	// 去除已遍历过的点
				tmpv.m_isInner)						// 去除内点
			{
				continue;
			}
			
			Vec3d vv = tmpv.m_xyz - v.m_xyz;
			double dist2 = dists[j] * 0.75f;	// sqrt
			double alpha = vv.dot(normal);
			alpha = alpha * alpha;
			if (alpha > dist2)		// 去除与法向量夹角小于30度或大于150度的点
			{
				continue;
			}
			Vec3d proj = tmpv.m_xyz - alpha * normal;		// 投射到切平面
			tmpvVector.push_back(Vertex(proj, v.m_neighbors[j]));
		}
		if (tmpvVector.size() < 3)	// 少于3个不能构成三角形
		{
			mesh.pushTriBeginIndex((int)triSet.size());
			continue;
		}

		// 将切平面转换为x-y平面进行三角形计算
		vVector.push_back(Vertex(Vec3d(0, 0, 0), mesh.m_curIndex));	// 原点
		Vec3d vx = tmpvVector[0].m_xyz - v.m_xyz;		// x轴
		vx = normalize(vx);
		for (int j = 0; j < tmpvVector.size(); j++)
		{
			Vec3d vv = tmpvVector[j].m_xyz - v.m_xyz;
			double x = vv.dot(vx);
			double y = vx.cross(vv)[id] / normal[id];
			Vec3d proj(x, y, 0);
			vVector.push_back(Vertex(proj, tmpvVector[j].m_index));
		}

		TriangleVector tVector;
		computeDelaunay(vVector, tVector);
// 		cout << vVector.size() << " " << tVector.size() << endl; 
// 		drawTrianglesOnPlane(tVector);
		for (int j = 0; j < tVector.size(); j++)
		{
			Triangle t = tVector[j];
//.........这里部分代码省略.........

void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
{

    // This routine takes any vector as argument but normalized 
    // vectors are necessary, if only for computing the dot product.
    // Too bad the API is that generic, it leads to performance loss.
    // Even in the case the 2 vectors are not normalized but same length,
    // the sqrt could be shared, but we have no way to know beforehand
    // at this point, while the caller may know.
    // So, we have to test... in the hope of saving at least a sqrt
    Vec3d sourceVector = from;
    Vec3d targetVector = to;
    
    value_type fromLen2 = length2(from);
    value_type fromLen;
    // normalize only when necessary, epsilon test
    if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) {
      fromLen = sqrt(fromLen2);
      sourceVector = Vec3d(sourceVector[0]/fromLen,
			   sourceVector[1]/fromLen,
			   sourceVector[2]/fromLen);
    } else fromLen = 1.0;
    
    value_type toLen2 = length2(to);
    // normalize only when necessary, epsilon test
    if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
        value_type toLen;
        // re-use fromLen for case of mapping 2 vectors of the same length
        if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) {
            toLen = fromLen;
        } 
        else toLen = sqrt(toLen2);
	targetVector = Vec3d(targetVector[0]/toLen,
			   targetVector[1]/toLen,
			   targetVector[2]/toLen);

    }

    
    // Now let's get into the real stuff
    // Use "dot product plus one" as test as it can be re-used later on
    double dotProdPlus1 = 1.0 + sourceVector.dot( targetVector);
    
    // Check for degenerate case of full u-turn. Use epsilon for detection
    if (dotProdPlus1 < 1e-7) {
    
        // Get an orthogonal vector of the given vector
        // in a plane with maximum vector coordinates.
        // Then use it as quaternion axis with pi angle
        // Trick is to realize one value at least is >0.6 for a normalized vector.
        if (fabs(sourceVector[0]) < 0.6) {
            const double norm = sqrt(1.0 - sourceVector[0] * sourceVector[0]);
            _v[0] = 0.0; 
            _v[1] = sourceVector[2] / norm;
            _v[2] = -sourceVector[1] / norm;
            _v[3] = 0.0;
        } else if (fabs(sourceVector[1]) < 0.6) {
            const double norm = sqrt(1.0 - sourceVector[1] * sourceVector[1]);
            _v[0] = -sourceVector[2] / norm;
            _v[1] = 0.0;
            _v[2] = sourceVector[0] / norm;
            _v[3] = 0.0;
        } else {
            const double norm = sqrt(1.0 - sourceVector[2] * sourceVector[2]);
            _v[0] = sourceVector[1] / norm;
            _v[1] = -sourceVector[0] / norm;
            _v[2] = 0.0;
            _v[3] = 0.0;
        }
    }
    
    else {
        // Find the shortest angle quaternion that transforms normalized vectors
        // into one other. Formula is still valid when vectors are colinear
        const double s = sqrt(0.5 * dotProdPlus1);
         Vec3d tmp = sourceVector.cross( targetVector);
	tmp= Vec3d(tmp[0]/ (2.0*s), tmp[1]/ (2.0*s), tmp[2]/ (2.0*s));
        _v[0] = tmp[0];
        _v[1] = tmp[1];
        _v[2] = tmp[2];
        _v[3] = s;
    }
}