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 ¢er, 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]; t.m_vertices[0] = mesh.getVertex(t.m_vertices[0].m_index); t.m_vertices[1] = mesh.getVertex(t.m_vertices[1].m_index); t.m_vertices[2] = mesh.getVertex(t.m_vertices[2].m_index); triSet.insert(t); } mesh.pushTriBeginIndex((int)triSet.size()); } for (TriangleSet::iterator iter = triSet.begin(); iter != triSet.end(); iter++) { mesh.m_triangles.push_back(*iter); } }
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; } }