OSG_BASE_DLLMAPPING bool MatrixLookAt(OSG::Matrix &result, OSG::Real32 fromx, OSG::Real32 fromy, OSG::Real32 fromz, OSG::Real32 atx, OSG::Real32 aty, OSG::Real32 atz, OSG::Real32 upx, OSG::Real32 upy, OSG::Real32 upz) { Vec3f view; Vec3f right; Vec3f newup; Vec3f up; view.setValues(fromx - atx , fromy - aty, fromz - atz); view.normalize(); up.setValues(upx, upy, upz); right = up.cross(view); if(right.dot(right) < TypeTraits<Real32>::getDefaultEps()) { return true; } right.normalize(); newup = view.cross(right); result.setIdentity (); result.setTranslate(fromx, fromy, fromz); Matrix tmpm; tmpm.setValue(right, newup, view); result.mult(tmpm); return false; }
inline Vec3f Mesh::triangleNormal(const Face *f){ Vec3f v0, v1, v2; v0 = getVertex(f->vertices[0])->v; v1 = getVertex(f->vertices[1])->v; v2 = getVertex(f->vertices[2])->v; Vec3f a = v1 - v0; Vec3f b = v2 - v0; Vec3f c = a.cross(b); Vec3f d = (c.isZero())? zeroVec3f : c.normalize(); return d; }
Vec3f radiationVector_qmc(const Vec3f& w, Qmc& random) { Vec3f u = (std::abs(w.x()) > 0.0001) ? Vec3f::UnitY().cross(w).normalized() : Vec3f::UnitX().cross(w).normalized(); Vec3f v = w.cross(u); const Real r1 = 2.0 * M_PI * random.next(); const Real r2 = random.next(); const Real r2s = std::sqrt(r2); return (u * std::cos(r1) * r2s + v * std::sin(r1) * r2s + w * std::sqrt(1.0 - r2)).normalized(); }
/** Rotate the object around its x axis **/ void VRTransform::rotateX(float a) {//rotate around x axis Vec3f dir = _at - _from; Vec3f d = dir.cross(_up); d.normalize(); Quaternion q = Quaternion(d, a); q.multVec(_up,_up); q.multVec(dir,dir); _at = _from + dir; reg_change(); //cout << "\nRotating " << name << " " << a ; }
void Renderer::orbit(Vec2f delta) { // printf("delta = %f, %f\n", delta.x, delta.y); Vec3f u, v, w; w = _up; w.normalize(); Vec3f up = _nonParallelVector(w); u = up.cross(w); u.normalize(); v = w.cross(u); Matrix4f basis; basis.identity(); basis.c[0][0] = u.x; basis.c[0][1] = u.y; basis.c[0][2] = u.z; basis.c[1][0] = v.x; basis.c[1][1] = v.y; basis.c[1][2] = v.z; basis.c[2][0] = w.x; basis.c[2][1] = w.y; basis.c[2][2] = w.z; Matrix4f basisInv = basis.inverted(); Vec3f newEye = basisInv * _eye; float r = newEye.length(); float phi = atan2f(newEye.y, newEye.x); float theta = asinf(newEye.z / r); // increment phi and theta by mouse motion //printf("delta phi = %f\n", M_PI_2 * delta.x); //printf("delta theta = %f\n", M_PI_2 * delta.y); phi = phi - M_PI_2 * delta.x; theta = theta - M_PI_2 * delta.y; float thetaLimit = (float) (89 * M_PI / 180); if (theta > thetaLimit) theta = thetaLimit; if (theta < -thetaLimit) theta = -thetaLimit; newEye.x = r * cosf(theta) * cosf(phi); newEye.y = r * cosf(theta) * sinf(phi); newEye.z = r * sinf(theta); newEye = basis * newEye; printf("old eye = %f, %f, %f\n", _eye.x, _eye.y, _eye.z); printf("new eye = %f, %f, %f\n", newEye.x, newEye.y, newEye.z); lookat(newEye, _target, _up); }
Eigen::Matrix4f lookAt(const Vec3f& eye, const Vec3f& target, const Vec3f& up) { Vec3f a = eye - target; Vec3f z_ = a / a.norm(); Vec3f b = up.cross(z_); Vec3f x_ = b / b.norm(); Vec3f y_ = z_.cross(x_); Eigen::Matrix4f R; R(0, 0) = x_.x(); R(0, 1) = x_.y(); R(0, 2) = x_.z(); R(0, 3) = 0; R(1, 0) = y_.x(); R(1, 1) = y_.y(); R(1, 2) = y_.z(); R(1, 3) = 0; R(2, 0) = z_.x(); R(2, 1) = z_.y(); R(2, 2) = z_.z(); R(2, 3) = 0; R(3, 0) = 0; R(3, 1) = 0; R(3, 2) = 0; R(3, 3) = 1; Eigen::Matrix4f T; for (int y = 0; y < 4; y++) { for (int x = 0; x < 4; x++) { T(x, y) = 0; } } T(0, 0) = 1; T(0, 3) = -eye.x(); T(1, 1) = 1; T(1, 3) = -eye.y(); T(2, 2) = 1; T(2, 3) = -eye.z(); T(3, 3) = 1; return R * T; };
// ---------------------------------------------- // RayTriangle Intersection Function // by RealTime Rendering: // http://www.acm.org/jgt/papers/MollerTrumbore97/ // date: December 12th, 2000 // ---------------------------------------------- bool NormalQuantifier::rayTriangle ( const Vec3f & dir, const Vec3f & vert0, const Vec3f & vert1, const Vec3f & vert2 ) const { Vec3f edge1, edge2, tvec, pvec, qvec; float det,inv_det; float u,v; Vec3f orig(0,0,0); /* find vectors for two edges sharing vert0 */ edge1 = vert1 - vert0; edge2 = vert2 - vert0; /* begin calculating determinant - also used to calculate U parameter */ pvec = dir.cross(edge2); /* if determinant is near zero, ray lies in plane of triangle */ det = edge1.dot(pvec); /* the non-culling branch */ if (det > -EPSILON && det < EPSILON) return false; inv_det = 1.0f / det; /* calculate distance from vert0 to ray origin */ tvec = orig - vert0; /* calculate U parameter and test bounds */ u = tvec.dot(pvec) *inv_det; if (u < 0.0 || u > 1.0) return false; /* prepare to test V parameter */ qvec = tvec.cross(edge1); /* calculate V parameter and test bounds */ v = dir.dot(qvec) * inv_det; if (v < 0.0 || u + v > 1.0) return false; /* calculate t, ray intersects triangle */ //t = edge2.dot(qvec) * inv_det; return true; };
bool FormTriangle::rayTest(const Vec3f &start, const Vec3f &dir, float &t, Vec3f &hitPos) { Vec3f e1 = _points[1] - _points[0]; Vec3f e2 = _points[2] - _points[0]; Vec3f h = dir.cross(e2); float a = e1.dot(h); if(abs(a) < 0.00001f) return false; float f = 1.0f / a; Vec3f s = start - _points[0]; float u = f * s.dot(h); if(u < 0.0f || u > 1.0f) return false; Vec3f q = s.cross(e1); float v = f * dir.dot(q); if(v < 0.0f || u + v > 1.0f) return false; t = f * e2.dot(q); if(t > 0.00001f) { hitPos = start + dir * t; return true; } return false; }
void Streamer::draw() { glBegin( GL_TRIANGLE_STRIP ); for( int i=0; i<mLen-1; i++ ){ Vec3f p1 = mPositions[i]; Vec3f p2 = mPositions[i+1]; Vec3f dir = p2 - p1; dir.normalize(); Vec3f perp1 = dir.cross( Vec3f::yAxis() ); perp1.normalize(); gl::vertex( p1 - perp1 * 2.0f * mAgePer ); gl::vertex( p1 + perp1 * 2.0f * mAgePer ); } glEnd(); }
static void gramSchmidt(Vec3f &a, Vec3f &b, Vec3f &c) { a.normalize(); b -= a*a.dot(b); if (b.lengthSq() < 1e-5) b = randomOrtho(a); else b.normalize(); c -= a*a.dot(c); c -= b*b.dot(c); if (c.lengthSq() < 1e-5) c = a.cross(b); else c.normalize(); }
void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vp, const Vec3f& L, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c) { Vec3f vbva = va - vb; Vec3f vbvp = vp - vb; Vec3f b0a0 = a0 - b0; Vec3f b0p0 = p0 - b0; Vec3f L_cross_vbvp = L.cross(vbvp); Vec3f L_cross_b0p0 = L.cross(b0p0); *a = L_cross_vbvp.dot(vbva); *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); *c = L_cross_b0p0.dot(b0a0); }
void Diver::debugDrawIndices(const CameraOrtho &camera){ static const float fontScale = 0.005f; Vec3f v; Vec3f w; Vec3f u; camera.getBillboardVectors(&w, &u); v = w.cross(u); const static Vec2f zero; const gl::TextureFontRef& sharedTextureFont = SharedTextureFont::Get(); float fontDescent = sharedTextureFont->getDescent(); Matrix44f mat; Matrix44f rot = Matrix44f::createRotationOnb(u,w,v); rot*= Matrix44f::createRotation(Vec3f::zAxis(), M_PI_2); rot*= Matrix44f::createScale(Vec3f(fontScale,fontScale,fontScale)); gl::enableAlphaTest(); gl::enableAlphaBlending(); glColor3f(1,1,1); int i = -1; while(++i < mPoints.size()){ mat.setToIdentity(); mat *= Matrix44f::createTranslation(mPoints[i]); mat *= rot; string stringTexCoord = toString(mTexcoords[i]); Vec2f stringSize = sharedTextureFont->measureString(stringTexCoord); glPushMatrix(); glMultMatrixf(&mat[0]); glColor4f(0,0,0,0.75f); gl::drawSolidRect(Rectf(0,fontDescent,stringSize.x, stringSize.y * -1+fontDescent)); glColor3f(1,1,1); sharedTextureFont->drawString(stringTexCoord, zero); glPopMatrix(); } gl::disableAlphaBlending(); gl::disableAlphaTest(); }
bool MoleculeCisTrans::sameline (const Vec3f &beg, const Vec3f &end, const Vec3f &nei_beg) { Vec3f norm_diff, norm_beg; norm_diff.diff(beg, end); if (!norm_diff.normalize()) return true; norm_beg.diff(nei_beg, beg); if (!norm_beg.normalize()) return true; Vec3f cross; cross.cross(norm_diff, norm_beg); float sin_angle = cross.lengthSqr(); if (fabs(sin_angle) < 0.01) return true; return false; }
void ColorCubePoints::LineSolver::solve(vector<Vec3f> &points) { const int m = points.size(); // Number of functions to minimize const int n = 5; // Number of independent variables vector<int> iwa(n); // Integer work array vector<float> wa(m*n + 5*n + m); // Working array vector<float> residuals(m); // Default tolerance: sqaure root of machine precision float tol = sqrt(sdpmpar(1)); // If solution is out of range, start over float limit0 = 4.0f; if (result.x0 < -limit0 || result.x0 > limit0 || result.y0 < -limit0 || result.y0 > limit0) { result.setDefault(); } // Minimize the system of equations slmdif1(&minFunc, &points[0], m, n, &result.array[0], &residuals[0], tol, &iwa[0], &wa[0], (int)wa.size()); // Local coordinate system has a stable XY plane perpendicular to the line, and Z along the line. // X and Y axes are defined relative to Z, to be the same length. Vec3f origin(result.x0, result.y0, 0.0f); Vec3f zAxis(result.xz, result.yz, 1.0f); float zScale = 1.0f / zAxis.length(); Vec3f up(0.0f, 1.0f, 0.0f); Vec3f xAxis = zAxis.cross(up).normalized() * zScale; Vec3f yAxis = xAxis.cross(zAxis).normalized() * zScale; localToWorld.setColumn(0, Vec4f(xAxis, origin.x)); localToWorld.setColumn(1, Vec4f(yAxis, origin.y)); localToWorld.setColumn(2, Vec4f(zAxis, origin.z)); localToWorld.setColumn(3, Vec4f(0.0f, 0.0f, 0.0f, 1.0f)); worldToLocal = localToWorld.affineInverted(); }
void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d) { Vec3f vavb = vb - va; Vec3f vcvd = vd - vc; Vec3f vavc = vc - va; Vec3f c0d0 = d0 - c0; Vec3f a0b0 = b0 - a0; Vec3f a0c0 = c0 - a0; Vec3f vavb_cross_vcvd = vavb.cross(vcvd); Vec3f vavb_cross_c0d0 = vavb.cross(c0d0); Vec3f a0b0_cross_vcvd = a0b0.cross(vcvd); Vec3f a0b0_cross_c0d0 = a0b0.cross(c0d0); *a = vavc.dot(vavb_cross_vcvd); *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); *d = a0c0.dot(a0b0_cross_c0d0); }
void FastTrailsApp::update() { // find out how many trails we should add const double trails_per_second = 2000.0; double elapsed = getElapsedSeconds() - mTime; uint32_t num_trails = uint32_t(elapsed * trails_per_second); // add this number of trails // (note: it's an ugly function that draws a swirling trail around a sphere, just for demo purposes) for(size_t i=0; i<num_trails; ++i ) { float phi = mAngle * 0.01f; float prev_phi = phi - 0.01f; float theta = phi * 0.03f; float prev_theta = prev_phi * 0.03f; Vec3f pos = 45.0f * Vec3f( sinf( phi ) * cosf( theta ), sinf( phi ) * sinf( theta ), cosf( phi ) ); Vec3f prev_pos = 45.0f * Vec3f( sinf( prev_phi ) * cosf( prev_theta ), sinf( prev_phi ) * sinf( prev_theta ), cosf( prev_phi ) ); Vec3f direction = pos - prev_pos; Vec3f right = Vec3f( sinf( 20.0f * phi ), 0.0f, cosf( 20.0f * phi ) ); Vec3f normal = direction.cross( right ).normalized(); // add two vertices, one at each side of the center line mTrail.push_front( pos - 1.0f * normal ); mTrail.push_front( pos + 1.0f * normal ); mAngle += 1.0; } // keep trail length within bounds while( mTrail.size() > TRAIL_LENGTH ) mTrail.pop_back(); // copy to trail to vbo (there's probably a faster way than this, need to check that out later) gl::VboMesh::VertexIter itr = mVboMesh.mapVertexBuffer(); for( size_t i=0; i<mTrail.size(); ++i, ++itr ) itr.setPosition( mTrail[i] ); // advance time mTime += num_trails / trails_per_second; }
/// @brief Compute the cubic coefficients for VF case /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d) { Vec3f vavb = vb - va; Vec3f vavc = vc - va; Vec3f vavp = vp - va; Vec3f a0b0 = b0 - a0; Vec3f a0c0 = c0 - a0; Vec3f a0p0 = p0 - a0; Vec3f vavb_cross_vavc = vavb.cross(vavc); Vec3f vavb_cross_a0c0 = vavb.cross(a0c0); Vec3f a0b0_cross_vavc = a0b0.cross(vavc); Vec3f a0b0_cross_a0c0 = a0b0.cross(a0c0); *a = vavp.dot(vavb_cross_vavc); *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); *d = a0p0.dot(a0b0_cross_a0c0); }
void MainCamera::lookToTarget() { Vec3f z = getForward() / getForward().length(); Vec3f b = up_.cross(z); Vec3f x = b / b.length(); Vec3f y = z.cross(x); Eigen::Matrix4f R; R << x.x, x.y, x.z, 0.0f, y.x, y.y, y.z, 0.0f, z.x, z.y, z.z, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f; Eigen::Matrix4f T; T << 1.0f, 0.0f, 0.0f, -pos_.x, 0.0f, 1.0f, 0.0f, -pos_.y, 0.0f, 0.0f, 1.0f, -pos_.z, 0.0f, 0.0f, 0.0f, 1.0f; Eigen::Matrix4f m = R * T; glMultMatrixf(m.data()); }
FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q) { // Compute vectors along the 6 sides Vec3f Sv[3]; Vec3f Tv[3]; Vec3f VEC; Sv[0] = S[1] - S[0]; Sv[1] = S[2] - S[1]; Sv[2] = S[0] - S[2]; Tv[0] = T[1] - T[0]; Tv[1] = T[2] - T[1]; Tv[2] = T[0] - T[2]; // For each edge pair, the vector connecting the closest points // of the edges defines a slab (parallel planes at head and tail // enclose the slab). If we can show that the off-edge vertex of // each triangle is outside of the slab, then the closest points // of the edges are the closest points for the triangles. // Even if these tests fail, it may be helpful to know the closest // points found, and whether the triangles were shown disjoint Vec3f V, Z, minP, minQ; FCL_REAL mindd; int shown_disjoint = 0; mindd = (S[0] - T[0]).sqrLength() + 1; // Set first minimum safely high for(int i = 0; i < 3; ++i) { for(int j = 0; j < 3; ++j) { // Find closest points on edges i & j, plus the // vector (and distance squared) between these points segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); V = Q - P; FCL_REAL dd = V.dot(V); // Verify this closest point pair only if the distance // squared is less than the minimum found thus far. if(dd <= mindd) { minP = P; minQ = Q; mindd = dd; Z = S[(i+2)%3] - P; FCL_REAL a = Z.dot(VEC); Z = T[(j+2)%3] - Q; FCL_REAL b = Z.dot(VEC); if((a <= 0) && (b >= 0)) return sqrt(dd); FCL_REAL p = V.dot(VEC); if(a < 0) a = 0; if(b > 0) b = 0; if((p - a + b) > 0) shown_disjoint = 1; } } } // No edge pairs contained the closest points. // either: // 1. one of the closest points is a vertex, and the // other point is interior to a face. // 2. the triangles are overlapping. // 3. an edge of one triangle is parallel to the other's face. If // cases 1 and 2 are not true, then the closest points from the 9 // edge pairs checks above can be taken as closest points for the // triangles. // 4. possibly, the triangles were degenerate. When the // triangle points are nearly colinear or coincident, one // of above tests might fail even though the edges tested // contain the closest points. // First check for case 1 Vec3f Sn; FCL_REAL Snl; Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle Snl = Sn.dot(Sn); // Compute square of length of normal // If cross product is long enough, if(Snl > 1e-15) { // Get projection lengths of T points Vec3f Tp; V = S[0] - T[0]; Tp[0] = V.dot(Sn); V = S[0] - T[1]; Tp[1] = V.dot(Sn); V = S[0] - T[2]; Tp[2] = V.dot(Sn); // If Sn is a separating direction, // find point with smallest projection int point = -1; if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) { if(Tp[0] < Tp[1]) point = 0; else point = 1; if(Tp[2] < Tp[point]) point = 2; } else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) { if(Tp[0] > Tp[1]) point = 0; else point = 1; if(Tp[2] > Tp[point]) point = 2; } // If Sn is a separating direction, if(point >= 0) { shown_disjoint = 1; // Test whether the point found, when projected onto the // other triangle, lies within the face. V = T[point] - S[0]; Z = Sn.cross(Sv[0]); if(V.dot(Z) > 0) { V = T[point] - S[1]; Z = Sn.cross(Sv[1]); if(V.dot(Z) > 0) { V = T[point] - S[2]; Z = Sn.cross(Sv[2]); if(V.dot(Z) > 0) { // T[point] passed the test - it's a closest point for // the T triangle; the other point is on the face of S P = T[point] + Sn * (Tp[point] / Snl); Q = T[point]; return (P - Q).length(); } } } } } Vec3f Tn; FCL_REAL Tnl; Tn = Tv[0].cross(Tv[1]); Tnl = Tn.dot(Tn); if(Tnl > 1e-15) { Vec3f Sp; V = T[0] - S[0]; Sp[0] = V.dot(Tn); V = T[0] - S[1]; Sp[1] = V.dot(Tn); V = T[0] - S[2]; Sp[2] = V.dot(Tn); int point = -1; if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) { if(Sp[0] < Sp[1]) point = 0; else point = 1; if(Sp[2] < Sp[point]) point = 2; } else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) { if(Sp[0] > Sp[1]) point = 0; else point = 1; if(Sp[2] > Sp[point]) point = 2; } if(point >= 0) { shown_disjoint = 1; V = S[point] - T[0]; Z = Tn.cross(Tv[0]); if(V.dot(Z) > 0) { V = S[point] - T[1]; Z = Tn.cross(Tv[1]); if(V.dot(Z) > 0) { V = S[point] - T[2]; Z = Tn.cross(Tv[2]); if(V.dot(Z) > 0) { P = S[point]; Q = S[point] + Tn * (Sp[point] / Tnl); return (P - Q).length(); } } } } } // Case 1 can't be shown. // If one of these tests showed the triangles disjoint, // we assume case 3 or 4, otherwise we conclude case 2, // that the triangles overlap. if(shown_disjoint) { P = minP; Q = minQ; return sqrt(mindd); } else return 0; }
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); VEC = B.cross(TMP); } else { X = P + A * t; VEC = A.cross(B); if(VEC.dot(T) < 0) { VEC = VEC * (-1); } } } }
void qRansacSD::doAction() { assert(m_app); if (!m_app) return; const ccHObject::Container& selectedEntities = m_app->getSelectedEntities(); size_t selNum = selectedEntities.size(); if (selNum!=1) { m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } ccHObject* ent = selectedEntities[0]; assert(ent); if (!ent || !ent->isA(CC_POINT_CLOUD)) { m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } ccPointCloud* pc = static_cast<ccPointCloud*>(ent); //input cloud size_t count = (size_t)pc->size(); bool hasNorms = pc->hasNormals(); PointCoordinateType bbMin[3],bbMax[3]; pc->getBoundingBox(bbMin,bbMax); //Convert CC point cloud to RANSAC_SD type PointCloud cloud; { try { cloud.reserve(count); } catch(...) { m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } //default point & normal Point Pt; Pt.normal[0] = 0.0; Pt.normal[1] = 0.0; Pt.normal[2] = 0.0; for (unsigned i=0; i<(unsigned)count; ++i) { const CCVector3* P = pc->getPoint(i); Pt.pos[0] = P->x; Pt.pos[1] = P->y; Pt.pos[2] = P->z; if (hasNorms) { const PointCoordinateType* N = pc->getPointNormal(i); Pt.normal[0] = N[0]; Pt.normal[1] = N[1]; Pt.normal[2] = N[2]; } cloud.push_back(Pt); } //manually set bounding box! Vec3f cbbMin,cbbMax; cbbMin[0] = bbMin[0]; cbbMin[1] = bbMin[1]; cbbMin[2] = bbMin[2]; cbbMax[0] = bbMax[0]; cbbMax[1] = bbMax[1]; cbbMax[2] = bbMax[2]; cloud.setBBox(cbbMin,cbbMax); } //cloud scale (useful for setting several parameters const float scale = cloud.getScale(); //init dialog with default values ccRansacSDDlg rsdDlg(m_app->getMainWindow()); rsdDlg.epsilonDoubleSpinBox->setValue(.01f * scale); // set distance threshold to .01f of bounding box width // NOTE: Internally the distance threshold is taken as 3 * ransacOptions.m_epsilon!!! rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.02f * scale); // set bitmap resolution to .02f of bounding box width // NOTE: This threshold is NOT multiplied internally! rsdDlg.supportPointsSpinBox->setValue(s_supportPoints); rsdDlg.normThreshDoubleSpinBox->setValue(s_normThresh); rsdDlg.probaDoubleSpinBox->setValue(s_proba); rsdDlg.planeCheckBox->setChecked(s_primEnabled[0]); rsdDlg.sphereCheckBox->setChecked(s_primEnabled[1]); rsdDlg.cylinderCheckBox->setChecked(s_primEnabled[2]); rsdDlg.coneCheckBox->setChecked(s_primEnabled[3]); rsdDlg.torusCheckBox->setChecked(s_primEnabled[4]); if (!rsdDlg.exec()) return; //for parameters persistence { s_supportPoints = rsdDlg.supportPointsSpinBox->value(); s_normThresh = rsdDlg.normThreshDoubleSpinBox->value(); s_proba = rsdDlg.probaDoubleSpinBox->value(); //consistency check { unsigned char primCount = 0; for (unsigned char k=0; k<5; ++k) primCount += (unsigned)s_primEnabled[k]; if (primCount==0) { m_app->dispToConsole("No primitive type selected!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } } s_primEnabled[0] = rsdDlg.planeCheckBox->isChecked(); s_primEnabled[1] = rsdDlg.sphereCheckBox->isChecked(); s_primEnabled[2] = rsdDlg.cylinderCheckBox->isChecked(); s_primEnabled[3] = rsdDlg.coneCheckBox->isChecked(); s_primEnabled[4] = rsdDlg.torusCheckBox->isChecked(); } //import parameters from dialog RansacShapeDetector::Options ransacOptions; { ransacOptions.m_epsilon = rsdDlg.epsilonDoubleSpinBox->value(); ransacOptions.m_bitmapEpsilon = rsdDlg.bitmapEpsilonDoubleSpinBox->value(); ransacOptions.m_normalThresh = rsdDlg.normThreshDoubleSpinBox->value(); ransacOptions.m_minSupport = rsdDlg.supportPointsSpinBox->value(); ransacOptions.m_probability = rsdDlg.probaDoubleSpinBox->value(); } //progress dialog ccProgressDialog progressCb(false,m_app->getMainWindow()); progressCb.setRange(0,0); if (!hasNorms) { progressCb.setInfo("Computing normals (please wait)"); progressCb.start(); QApplication::processEvents(); cloud.calcNormals(.01f * scale); if (pc->reserveTheNormsTable()) { for (size_t i=0; i<count; ++i) { CCVector3 N(cloud[i].normal); N.normalize(); pc->addNorm(N.u); } pc->showNormals(true); //currently selected entities appearance may have changed! pc->prepareDisplayForRefresh_recursive(); } else { m_app->dispToConsole("Not enough memory to compute normals!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } } // set which primitives are to be detected by adding the respective constructors RansacShapeDetector detector(ransacOptions); // the detector object if (rsdDlg.planeCheckBox->isChecked()) detector.Add(new PlanePrimitiveShapeConstructor()); if (rsdDlg.sphereCheckBox->isChecked()) detector.Add(new SpherePrimitiveShapeConstructor()); if (rsdDlg.cylinderCheckBox->isChecked()) detector.Add(new CylinderPrimitiveShapeConstructor()); if (rsdDlg.coneCheckBox->isChecked()) detector.Add(new ConePrimitiveShapeConstructor()); if (rsdDlg.torusCheckBox->isChecked()) detector.Add(new TorusPrimitiveShapeConstructor()); size_t remaining = count; typedef std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > DetectedShape; MiscLib::Vector< DetectedShape > shapes; // stores the detected shapes // run detection // returns number of unassigned points // the array shapes is filled with pointers to the detected shapes // the second element per shapes gives the number of points assigned to that primitive (the support) // the points belonging to the first shape (shapes[0]) have been sorted to the end of pc, // i.e. into the range [ pc.size() - shapes[0].second, pc.size() ) // the points of shape i are found in the range // [ pc.size() - \sum_{j=0..i} shapes[j].second, pc.size() - \sum_{j=0..i-1} shapes[j].second ) { progressCb.setInfo("Operation in progress"); progressCb.setMethodTitle("Ransac Shape Detection"); progressCb.start(); //run in a separate thread s_detector = &detector; s_shapes = &shapes; s_cloud = &cloud; QFuture<void> future = QtConcurrent::run(doDetection); unsigned progress = 0; while (!future.isFinished()) { #if defined(_WIN32) || defined(WIN32) ::Sleep(500); #else sleep(500); #endif progressCb.update(++progress); //Qtconcurrent::run can't be canceled! /*if (progressCb.isCancelRequested()) { future.cancel(); future.waitForFinished(); s_remainingPoints = count; break; } //*/ } remaining = s_remainingPoints; progressCb.stop(); QApplication::processEvents(); } //else //{ // remaining = detector.Detect(cloud, 0, cloud.size(), &shapes); //} #ifdef _DEBUG FILE* fp = fopen("RANS_SD_trace.txt","wt"); fprintf(fp,"[Options]\n"); fprintf(fp,"epsilon=%f\n",ransacOptions.m_epsilon); fprintf(fp,"bitmap epsilon=%f\n",ransacOptions.m_bitmapEpsilon); fprintf(fp,"normal thresh=%f\n",ransacOptions.m_normalThresh); fprintf(fp,"min support=%i\n",ransacOptions.m_minSupport); fprintf(fp,"probability=%f\n",ransacOptions.m_probability); fprintf(fp,"\n[Statistics]\n"); fprintf(fp,"input points=%i\n",count); fprintf(fp,"segmented=%i\n",count-remaining); fprintf(fp,"remaining=%i\n",remaining); if (shapes.size()>0) { fprintf(fp,"\n[Shapes]\n"); for (unsigned i=0; i<shapes.size(); ++i) { PrimitiveShape* shape = shapes[i].first; unsigned shapePointsCount = shapes[i].second; std::string desc; shape->Description(&desc); fprintf(fp,"#%i - %s - %i points\n",i+1,desc.c_str(),shapePointsCount); } } fclose(fp); #endif if (remaining == count) { m_app->dispToConsole("Segmentation failed...",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } if (shapes.size() > 0) { ccHObject* group = 0; for (MiscLib::Vector<DetectedShape>::const_iterator it = shapes.begin(); it != shapes.end(); ++it) { const PrimitiveShape* shape = it->first; size_t shapePointsCount = it->second; //too many points?! if (shapePointsCount > count) { m_app->dispToConsole("Inconsistent result!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); break; } std::string desc; shape->Description(&desc); //new cloud for sub-part ccPointCloud* pcShape = new ccPointCloud(desc.c_str()); //we fill cloud with sub-part points if (!pcShape->reserve((unsigned)shapePointsCount)) { m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); delete pcShape; break; } bool saveNormals = pcShape->reserveTheNormsTable(); for (size_t j=0; j<shapePointsCount; ++j) { pcShape->addPoint(CCVector3(cloud[count-1-j].pos)); if (saveNormals) pcShape->addNorm(cloud[count-1-j].normal); } //random color unsigned char col[3]= { (unsigned char)(255.0*(float)rand()/(float)RAND_MAX), (unsigned char)(255.0*(float)rand()/(float)RAND_MAX), 0 }; col[2]=255-(col[1]+col[2])/2; pcShape->setRGBColor(col); pcShape->showColors(true); pcShape->showNormals(saveNormals); pcShape->setVisible(true); //convert detected primitive into a CC primitive type ccGenericPrimitive* prim = 0; switch(shape->Identifier()) { case 0: //plane { const PlanePrimitiveShape* plane = static_cast<const PlanePrimitiveShape*>(shape); Vec3f G = plane->Internal().getPosition(); Vec3f N = plane->Internal().getNormal(); Vec3f X = plane->getXDim(); Vec3f Y = plane->getYDim(); //we look for real plane extents float minX,maxX,minY,maxY; for (size_t j=0; j<shapePointsCount; ++j) { std::pair<float,float> param; plane->Parameters(cloud[count-1-j].pos,¶m); if (j!=0) { if (minX<param.first) minX=param.first; else if (maxX>param.first) maxX=param.first; if (minY<param.second) minY=param.second; else if (maxY>param.second) maxY=param.second; } else { minX=maxX=param.first; minY=maxY=param.second; } } //we recenter plane (as it is not always the case!) float dX = maxX-minX; float dY = maxY-minY; G += X * (minX+dX*0.5); G += Y * (minY+dY*0.5); //we build matrix from these vecctors ccGLMatrix glMat(CCVector3(X.getValue()), CCVector3(Y.getValue()), CCVector3(N.getValue()), CCVector3(G.getValue())); //plane primitive prim = new ccPlane(dX,dY,&glMat); } break; case 1: //sphere { const SpherePrimitiveShape* sphere = static_cast<const SpherePrimitiveShape*>(shape); float radius = sphere->Internal().Radius(); Vec3f CC = sphere->Internal().Center(); pcShape->setName(QString("Sphere (r=%1)").arg(radius,0,'f')); //we build matrix from these vecctors ccGLMatrix glMat; glMat.setTranslation(CCVector3(CC.getValue())); //sphere primitive prim = new ccSphere(radius,&glMat); prim->setEnabled(false); } break; case 2: //cylinder { const CylinderPrimitiveShape* cyl = static_cast<const CylinderPrimitiveShape*>(shape); Vec3f G = cyl->Internal().AxisPosition(); Vec3f N = cyl->Internal().AxisDirection(); Vec3f X = cyl->Internal().AngularDirection(); Vec3f Y = N.cross(X); float r = cyl->Internal().Radius(); float hMin = cyl->MinHeight(); float hMax = cyl->MaxHeight(); float h = hMax-hMin; G += N * (hMin+h*0.5); pcShape->setName(QString("Cylinder (r=%1/h=%2)").arg(r,0,'f').arg(h,0,'f')); //we build matrix from these vecctors ccGLMatrix glMat(CCVector3(X.getValue()), CCVector3(Y.getValue()), CCVector3(N.getValue()), CCVector3(G.getValue())); //cylinder primitive prim = new ccCylinder(r,h,&glMat); prim->setEnabled(false); } break; case 3: //cone { const ConePrimitiveShape* cone = static_cast<const ConePrimitiveShape*>(shape); Vec3f CC = cone->Internal().Center(); Vec3f CA = cone->Internal().AxisDirection(); float alpha = cone->Internal().Angle(); //compute max height CCVector3 maxP(CC.getValue()); float maxHeight = 0; for (size_t j=0; j<shapePointsCount; ++j) { float h = cone->Internal().Height(cloud[count-1-j].pos); if (h>maxHeight) { maxHeight=h; maxP = CCVector3(cloud[count-1-j].pos); } } pcShape->setName(QString("Cone (alpha=%1/h=%2)").arg(alpha,0,'f').arg(maxHeight,0,'f')); float radius = tan(alpha)*maxHeight; CCVector3 Z = CCVector3(CA.getValue()); CCVector3 C = CCVector3(CC.getValue()); //cone apex //construct remaining of base Z.normalize(); CCVector3 X = maxP - (C + maxHeight * Z); X.normalize(); CCVector3 Y = Z * X; //we build matrix from these vecctors ccGLMatrix glMat(X,Y,Z,C+(maxHeight*0.5)*Z); //cone primitive prim = new ccCone(0,radius,maxHeight,0,0,&glMat); prim->setEnabled(false); } break; case 4: //torus { const TorusPrimitiveShape* torus = static_cast<const TorusPrimitiveShape*>(shape); if (torus->Internal().IsAppleShaped()) { m_app->dispToConsole("[qRansacSD] Apple-shaped torus are not handled by CloudCompare!",ccMainAppInterface::WRN_CONSOLE_MESSAGE); } else { Vec3f CC = torus->Internal().Center(); Vec3f CA = torus->Internal().AxisDirection(); float minRadius = torus->Internal().MinorRadius(); float maxRadius = torus->Internal().MajorRadius(); pcShape->setName(QString("Torus (r=%1/R=%2)").arg(minRadius,0,'f').arg(maxRadius,0,'f')); CCVector3 Z = CCVector3(CA.getValue()); CCVector3 C = CCVector3(CC.getValue()); //construct remaining of base CCVector3 X = Z.orthogonal(); CCVector3 Y = Z * X; //we build matrix from these vecctors ccGLMatrix glMat(X,Y,Z,C); //torus primitive prim = new ccTorus(maxRadius-minRadius,maxRadius+minRadius,M_PI*2.0,false,0,&glMat); prim->setEnabled(false); } } break; } //is there a primitive to add to part cloud? if (prim) { prim->applyGLTransformation_recursive(); pcShape->addChild(prim); prim->setDisplay(pcShape->getDisplay()); prim->setColor(col); prim->showColors(true); prim->setVisible(true); } if (!group) { group = new ccHObject(QString("Ransac Detected Shapes (%1)").arg(ent->getName())); m_app->addToDB(group,true,0,false); } group->addChild(pcShape); m_app->addToDB(pcShape,true,0,false); count -= shapePointsCount; QApplication::processEvents(); } if (group) { assert(group->getChildrenNumber()!=0); //we hide input cloud pc->setEnabled(false); m_app->dispToConsole("[qRansacSD] Input cloud has been automtically hidden!",ccMainAppInterface::WRN_CONSOLE_MESSAGE); //we add new group to DB/display group->setVisible(true); group->setDisplay_recursive(pc->getDisplay()); group->prepareDisplayForRefresh_recursive(); m_app->refreshAll(); } } }
void Tube::buildFrenet() { mFrames.clear(); int n = mPs.size(); mFrames.resize( n ); for( int i = 0; i < n; ++i ) { Vec3f p0, p1, p2; if( i < (n - 2) ) { p0 = mPs[i]; p1 = mPs[i + 1]; p2 = mPs[i + 2]; } else if( i == (n - 2) ) { p0 = mPs[i - 1]; p1 = mPs[i]; p2 = mPs[i + 1]; } else if( i == (n - 1) ) { p0 = mPs[i - 3]; p1 = mPs[i - 2]; p2 = mPs[i - 1]; } Vec3f t = (p1 - p0).normalized(); Vec3f n = t.cross(p2 - p0).normalized(); if( n.length() == 0.0f ) { int i = fabs( t[0] ) < fabs( t[1] ) ? 0 : 1; if( fabs( t[2] ) < fabs( t[i] ) ) i = 2; Vec3f v( 0.0f, 0.0f, 0.0f ); v[i] = 1.0; n = t.cross( v ).normalized(); } Vec3f b = t.cross( n ); Matrix44f& m = mFrames[i]; m.at( 0, 0 ) = b.x; m.at( 1, 0 ) = b.y; m.at( 2, 0 ) = b.z; m.at( 3, 0 ) = 0; m.at( 0, 1 ) = n.x; m.at( 1, 1 ) = n.y; m.at( 2, 1 ) = n.z; m.at( 3, 1 ) = 0; m.at( 0, 2 ) = t.x; m.at( 1, 2 ) = t.y; m.at( 2, 2 ) = t.z; m.at( 3, 2 ) = 0; m.at( 0, 3 ) = mPs[i].x; m.at( 1, 3 ) = mPs[i].y; m.at( 2, 3 ) = mPs[i].z; m.at( 3, 3 ) = 1; } }
Action::ResultE QuadParticleSystemDrawer::draw(DrawEnv *pEnv, ParticleSystemUnrecPtr System, const MFUInt32& Sort) { bool isSorted(Sort.size() > 0); UInt32 NumParticles; if(isSorted) { NumParticles = Sort.size(); } else { NumParticles = System->getNumParticles(); } Pnt3f P1,P2,P3,P4; UInt32 Index; //Calculate the CameraToObject basis Matrix WorldToObject(pEnv->getObjectToWorld()); WorldToObject.invert(); Matrix CameraToObject(pEnv->getCameraToWorld()); CameraToObject.mult(WorldToObject); glBegin(GL_QUADS); for(UInt32 i(0); i<NumParticles;++i) { if(isSorted) { Index = Sort[i]; } else { Index = i; } //Loop through all particles //Get The Normal of the Particle Vec3f Normal = getQuadNormal(pEnv, System, Index, CameraToObject); //Calculate the Binormal as the cross between Normal and Up Vec3f Binormal = getQuadUpDir(pEnv, System, Index, CameraToObject).cross(Normal); //Get the Up Direction of the Particle Vec3f Up = Normal.cross(Binormal); //Determine Local Space of the Particle //This is where error occurs Pnt3f Position = System->getPosition(Index); //Determine the Width and Height of the quad Real32 Width = System->getSize(Index).x()*getQuadSizeScaling().x(),Height =System->getSize(Index).y()*getQuadSizeScaling().y(); //Calculate Quads positions P1 = Position + (Width/2.0f)*Binormal + (Height/2.0f)*Up; P2 = Position + (Width/2.0f)*Binormal - (Height/2.0f)*Up; P3 = Position - (Width/2.0f)*Binormal - (Height/2.0f)*Up; P4 = Position - (Width/2.0f)*Binormal + (Height/2.0f)*Up; //Draw the Quad glNormal3fv(Normal.getValues()); glColor4fv(System->getColor(Index).getValuesRGBA()); glTexCoord2f(1.0, 1.0); glVertex3fv(P1.getValues()); glTexCoord2f(0.0, 1.0); glVertex3fv(P4.getValues()); glTexCoord2f(0.0, 0.0); glVertex3fv(P3.getValues()); glTexCoord2f(1.0, 0.0); glVertex3fv(P2.getValues()); } glColor4f(1.0f,1.0f,1.0f,1.0f); glEnd(); //Generate a local space for the particle return Action::Continue; }
bool CameraControls::handleEvent(const Window::Event& ev) { if (!m_commonControls) fail("CameraControls attached to a window without CommonControls!"); CommonControls& cc = *m_commonControls; FW_ASSERT(m_window == ev.window || ev.type == Window::EventType_AddListener); // Initialize movement. Mat3f orient = getOrientation(); Vec3f rotate = 0.0f; Vec3f move = 0.0f; // Handle events. switch (ev.type) { case Window::EventType_AddListener: FW_ASSERT(!m_window); m_window = ev.window; m_timer.unstart(); m_dragLeft = false; m_dragMiddle = false; m_dragRight = false; cc.addStateObject(this); addGUIControls(); repaint(); return false; case Window::EventType_RemoveListener: cc.removeStateObject(this); removeGUIControls(); repaint(); m_window = NULL; return false; case Window::EventType_KeyDown: if (ev.key == FW_KEY_MOUSE_LEFT) m_dragLeft = true; if (ev.key == FW_KEY_MOUSE_MIDDLE) m_dragMiddle = true; if (ev.key == FW_KEY_MOUSE_RIGHT) m_dragRight = true; if (ev.key == FW_KEY_WHEEL_UP) m_speed *= 1.2f; if (ev.key == FW_KEY_WHEEL_DOWN) m_speed /= 1.2f; break; case Window::EventType_KeyUp: if (ev.key == FW_KEY_MOUSE_LEFT) m_dragLeft = false; if (ev.key == FW_KEY_MOUSE_MIDDLE) m_dragMiddle = false; if (ev.key == FW_KEY_MOUSE_RIGHT) m_dragRight = false; break; case Window::EventType_Mouse: { Vec3f delta = Vec3f((F32)ev.mouseDelta.x, (F32)-ev.mouseDelta.y, 0.0f); if (m_dragLeft) rotate += delta * s_mouseRotateSpeed; if (m_dragMiddle) move += delta * m_speed * s_mouseStrafeSpeed; if (m_dragRight) move += Vec3f(0.0f, 0.0f, (F32)ev.mouseDelta.y) * m_speed * s_mouseStrafeSpeed; } break; case Window::EventType_Paint: { F32 timeDelta = m_timer.end(); F32 boost = cc.getKeyBoost(); Vec3f rotateTmp = 0.0f; bool alt = m_window->isKeyDown(FW_KEY_ALT); if (m_window->isKeyDown(FW_KEY_A) || (m_window->isKeyDown(FW_KEY_LEFT) && alt)) move.x -= 1.0f; if (m_window->isKeyDown(FW_KEY_D) || (m_window->isKeyDown(FW_KEY_RIGHT) && alt)) move.x += 1.0f; if (m_window->isKeyDown(FW_KEY_F) || m_window->isKeyDown(FW_KEY_PAGE_DOWN)) move.y -= 1.0f; if (m_window->isKeyDown(FW_KEY_R) || m_window->isKeyDown(FW_KEY_PAGE_UP)) move.y += 1.0f; if (m_window->isKeyDown(FW_KEY_W) || (m_window->isKeyDown(FW_KEY_UP) && alt)) move.z -= 1.0f; if (m_window->isKeyDown(FW_KEY_S) || (m_window->isKeyDown(FW_KEY_DOWN) && alt)) move.z += 1.0f; if (m_window->isKeyDown(FW_KEY_LEFT) && !alt) rotateTmp.x -= 1.0f; if (m_window->isKeyDown(FW_KEY_RIGHT) && !alt) rotateTmp.x += 1.0f; if (m_window->isKeyDown(FW_KEY_DOWN) && !alt) rotateTmp.y -= 1.0f; if (m_window->isKeyDown(FW_KEY_UP) && !alt) rotateTmp.y += 1.0f; if (m_window->isKeyDown(FW_KEY_E) || m_window->isKeyDown(FW_KEY_HOME)) rotateTmp.z -= 1.0f; if (m_window->isKeyDown(FW_KEY_Q) || m_window->isKeyDown(FW_KEY_INSERT)) rotateTmp.z += 1.0f; move *= timeDelta * m_speed * boost; rotate += rotateTmp * timeDelta * s_keyRotateSpeed * boost; } break; default: break; } // Apply movement. if (m_enableMovement) { if (!move.isZero()) m_position += orient * move; if (rotate.x != 0.0f || rotate.y != 0.0f) { Vec3f tmp = orient.col(2) * cos(rotate.x) - orient.col(0) * sin(rotate.x); m_forward = (orient.col(1) * sin(rotate.y) - tmp * cos(rotate.y)).normalized(); if (!m_keepAligned) m_up = (orient.col(1) * cos(rotate.y) + tmp * sin(rotate.y)).normalized(); else if (-m_forward.cross(m_up).dot(tmp.cross(m_up).normalized()) < s_inclinationLimit) m_forward = -tmp.normalized(); } if (rotate.z != 0.0f && !m_keepAligned) { Vec3f up = orient.transposed() * m_up; m_up = orient * Vec3f(up.x * cos(rotate.z) - sin(rotate.z), up.x * sin(rotate.z) + up.y * cos(rotate.z), up.z); } } // Apply alignment. if (m_alignY) m_up = Vec3f(0.0f, 1.0f, 0.0f); m_alignY = false; if (m_alignZ) m_up = Vec3f(0.0f, 0.0f, 1.0f); m_alignZ = false; // Update stereo mode. if (hasFeature(Feature_StereoControls)) { GLContext::Config config = m_window->getGLConfig(); config.isStereo = (m_enableStereo && GLContext::isStereoAvailable()); m_window->setGLConfig(config); } // Repaint continuously. if (ev.type == Window::EventType_Paint) repaint(); return false; }
void ProtoSpline3::parallelTransport() { // double up first and last verts //verts.insert(verts.begin(), verts.at(0)); //verts.push_back(verts.at(verts.size() - 1)); //frenetFrames.push_back(FrenetFrame(verts.at(0), Vec3f(1,0,0), Vec3f(0,-1,0), Vec3f(0,0,-1))); // add first vert // std::cout << "in createFrenetFrame(): verts.size() = " << verts.size() << std::endl; std::vector<Vec3f> tans; float theta = 0.0; Vec3f cp0, cp1, cp2; Vec3f tan, biNorm, norm, nextBiNorm, nextNorm; for (int i = 0; i < verts.size(); i++) { if (i == 0) { //cp0 = verts[verts.size() - 1]; cp0 = verts.at(i); cp1 = verts.at(i); cp2 = verts.at(i + 1); } else if (i == verts.size() - 1) { cp0 = verts.at(i - 1); cp1 = verts.at(i); cp2 = verts.at(i); // 0, circled back here? changed to i } else { cp0 = verts.at(i - 1); cp1 = verts.at(i); cp2 = verts.at(i + 1); //std::cout << "i = = " << i << std::endl; // std::cout << "cp0 " << cp0 << std::endl; // std::cout << "cp1 " << cp1 << std::endl; // std::cout << "cp2 " << cp2 << std::endl; // std::cout << "cross(cp1, cp2) " << cross(cp1, cp2) << std::endl; //std::cout << "cp2 " << cp2 << std::endl; } // fill tangents tan = cp2 - cp0; tan.normalize(); tans.push_back(tan); // collect initial frame if (i == 1) { // fix biNorm for parralel vectors biNorm = cp1.cross(cp2); // uh-oh parallel vecs // ! HACK ! avoids problems with orthonormal tubes. if (biNorm.mag() == 0) { if (cp1.x !=0 && cp2.x !=0){ biNorm = Vec3f(0, 1, 0); } if (cp1.y !=0 && cp2.y !=0){ biNorm = Vec3f(0, 0, -1); } if (cp1.z !=0 && cp2.z !=0){ biNorm = Vec3f(1, 0, 0); } } //std::cout << "biNorm pre = " << biNorm << std::endl; //std::cout << "biNorm.mag() = " << biNorm.mag() << std::endl; biNorm.normalize(); // biNorm.x = 1; // biNorm.y = 0; // biNorm.z = 0; // std::cout << "cp1 " << cp1 << std::endl; // std::cout << "cp2 " << cp2 << std::endl; // std::cout << "biNorm post = " << biNorm << std::endl; norm = biNorm.cross(tan); norm.normalize(); } } // rotate frame // std::cout << "tans.size() = " << tans.size() << std::endl; for (int i = 0; i < tans.size() - 1; i++) { if (biNorm.mag() == 0) { nextNorm = norm; //frenetFrames.push_back(FrenetFrame(verts.at(i), Vec3f(1,1,1), Vec3f(1,1,1), Vec3f(1,1,1))); // std::cout << "norm = " << norm << std::endl; } else { theta = acos(tans.at(i).dot(tans.at(i + 1))); Vec3f axis = tans.at(i); //std::cout << "tans.at(i + 1) = " << tans.at(i + 1) << std::endl; //std::cout << i << std::endl; axis = axis.cross(tans.at(i + 1)); //std::cout << "axis = " << axis << std::endl; axis.normalize(); ProtoMatrix3f m; nextNorm = m.getRotate(theta, axis, norm); //std::cout << "axis = " << axis << std::endl; nextBiNorm = tans.at(i + 1); nextBiNorm = nextBiNorm.cross(nextNorm); // std::cout << "nextNorm = " << nextNorm << std::endl; // std::cout << "norm = " << norm << std::endl; } //biNorm.normalize(); //norm.normalize(); // std::cout <<i<<std::endl; // std::cout << "tans.at(i) = " << tans.at(i) << std::endl; // std::cout << "biNorm = " << biNorm << std::endl; // std::cout << "norm = " << norm << std::endl; frenetFrames.push_back(ProtoFrenetFrame(verts.at(i), tans.at(i), biNorm, norm)); norm = nextNorm; biNorm = nextBiNorm; //std::cout << "verts = " << verts.at(i) << std::endl; } // std::cout << "in createFrenetFrame(): frenetFrames.size() = " << frenetFrames.size() << std::endl; }
// generate a bolt of lightning and add to our mesh // modified from Michael Hoffman's // (http://gamedevelopment.tutsplus.com/tutorials/how-to-generate-shockingly-good-2d-lightning-effects--gamedev-2681) void makeBolt(Vec3f source, Vec3f dest, int maxBranches = 0, float branchProb = 0.01, float wid = 0.05, int n = 80) { Vec3f tangent = (dest - source); // direction of lightning Vec3f normal = tangent.cross(Vec3f(0, 0, -1)).normalize(); // normal to lightning float length = tangent.mag(); // choose n random positions (0,1) along lightning vector and sort them vector<float> positions; positions.push_back(0); for (int i = 0; i < n; i++) positions.push_back(rnd::uniform()); sort(positions.begin(), positions.end()); float sway = 1; // max random walk step of displacement along normal float jaggedness = 1 / sway; Vec3f prevPoint = source; float prevDisplacement = 0; //float width = 0.06; float width = (wid > 0) ? wid : length / 25.0 + 0.01; //color = Color(1, 0.7, 1, 1); int branches = 0; mesh.primitive(Graphics::TRIANGLES); Vec3f point; for (int i = 1; i < n; i++) { float pos = positions[i]; // used to prevent sharp angles by ensuring very close positions also have // small perpendicular variation. float scale = (length * jaggedness) * (pos - positions[i - 1]); // defines an envelope. Points near the middle of the bolt can be further // from the central line. float envelope = pos > 0.95f ? mapRange(pos, 0.95f, 1.0f, 1.0f, 0.0f) : 1; // displacement from prevDisplacement (random walk (brownian motion)) float displacement = rnd::uniformS(sway) * scale + prevDisplacement; displacement *= envelope; // calculate point, source plus percentage along tangent, and displacement // along normal; point = source + pos * tangent + displacement * normal; // generate triangles (vertices and texture coordinates) from point and // prevPoint //********** mesh.vertex(prevPoint + normal * width); mesh.vertex(prevPoint - normal * width); mesh.vertex(point + normal * width); mesh.vertex(point + normal * width); mesh.vertex(prevPoint - normal * width); mesh.vertex(point - normal * width); mesh.texCoord(0, (float)(i - 1) / n); mesh.texCoord(1, (float)(i - 1) / n); mesh.texCoord(0, (float)(i) / n); mesh.texCoord(0, (float)(i) / n); mesh.texCoord(1, (float)(i - 1) / n); mesh.texCoord(1, (float)(i) / n); mesh.color(color); mesh.color(color); mesh.color(color); mesh.color(color); mesh.color(color); mesh.color(color); //********** if (branches < maxBranches && rnd::prob(branchProb)) { branches++; Vec3f dir(tangent); rotate(dir, Vec3f(0, 0, 1), rnd::uniformS(30) * M_DEG2RAD); dir.normalize(); float len = (dest - point).mag(); makeBolt(point, point + dir * len * 0.4, maxBranches - 1, branchProb, width * 0.6, n * 0.5); } // remember previous point and displacement for next iteration prevPoint = point; prevDisplacement = displacement; } //generate last piece mesh.vertex(point + normal * width); mesh.vertex(point - normal * width); mesh.vertex(dest + normal * width); mesh.vertex(dest + normal * width); mesh.vertex(point - normal * width); mesh.vertex(dest - normal * width); mesh.texCoord(0, (float)(n - 1) / n); mesh.texCoord(1, (float)(n - 1) / n); mesh.texCoord(0, (float)(n) / n); //0 mesh.texCoord(0, (float)(n) / n); //0 mesh.texCoord(1, (float)(n - 1) / n); mesh.texCoord(1, (float)(n) / n); //1 mesh.color(color); mesh.color(color); mesh.color(color); mesh.color(color); mesh.color(color); mesh.color(color); }
//Computes the normals, if they haven't been computed yet void computeNormals() { if (computedNormals) { return; } //Compute the rough version of the normals Vec3f** normals2 = new Vec3f*[l]; for (int i = 0; i < l; i++) { normals2[i] = new Vec3f[w]; } for (int z = 0; z < l; z++) { for (int x = 0; x < w; x++) { Vec3f sum(0.0f, 0.0f, 0.0f); Vec3f out; if (z > 0) { out = Vec3f(0.0f, hs[z - 1][x] - hs[z][x], -1.0f); } Vec3f in; if (z < l - 1) { in = Vec3f(0.0f, hs[z + 1][x] - hs[z][x], 1.0f); } Vec3f left; if (x > 0) { left = Vec3f(-1.0f, hs[z][x - 1] - hs[z][x], 0.0f); } Vec3f right; if (x < w - 1) { right = Vec3f(1.0f, hs[z][x + 1] - hs[z][x], 0.0f); } if (x > 0 && z > 0) { sum += out.cross(left).normalize(); } if (x > 0 && z < l - 1) { sum += left.cross(in).normalize(); } if (x < w - 1 && z < l - 1) { sum += in.cross(right).normalize(); } if (x < w - 1 && z > 0) { sum += right.cross(out).normalize(); } normals2[z][x] = sum; } } //Smooth out the normals const float FALLOUT_RATIO = 0.5f;//memperhalus for (int z = 0; z < l; z++) { for (int x = 0; x < w; x++) { Vec3f sum = normals2[z][x]; if (x > 0) { sum += normals2[z][x - 1] * FALLOUT_RATIO; } if (x < w - 1) { sum += normals2[z][x + 1] * FALLOUT_RATIO; } if (z > 0) { sum += normals2[z - 1][x] * FALLOUT_RATIO; } if (z < l - 1) { sum += normals2[z + 1][x] * FALLOUT_RATIO; } if (sum.magnitude() == 0) { sum = Vec3f(0.0f, 1.0f, 0.0f); } normals[z][x] = sum; } } for (int i = 0; i < l; i++) { delete[] normals2[i]; } delete[] normals2; computedNormals = true; }
void labelCorners(_Polygon (&Polygons)[6], int (&orderOfPolygons)[6], Point2f (&Corners)[24]) { Vec2f DA = Vec2f(Polygons[orderOfPolygons[0]].Center.x - Polygons[orderOfPolygons[3]].Center.x, Polygons[orderOfPolygons[0]].Center.y - Polygons[orderOfPolygons[3]].Center.y); DA = normalize(DA); Vec3f DA3; DA3.val[0] = DA.val[0]; DA3.val[1] = DA.val[1]; DA3.val[2] = 0; _Diagonal Diagonal; float angle, dist, dist2; int startingPoint[6], cornerLabel = 1, cornerIndex; for (int polygonIndex = 0; polygonIndex < 6; polygonIndex++) { Diagonal.Diag = Vec2f(Polygons[polygonIndex].Corners[0].x - Polygons[polygonIndex].Corners[2].x, Polygons[polygonIndex].Corners[0].y - Polygons[polygonIndex].Corners[2].y); Diagonal.indexOfCorner1 = 0; Diagonal.indexOfCorner2 = 2; angle = DA.dot(normalize(Diagonal.Diag)); if (abs(angle) < 0.95) { Diagonal.Diag = Vec2f(Polygons[polygonIndex].Corners[1].x - Polygons[polygonIndex].Corners[3].x, Polygons[polygonIndex].Corners[1].y - Polygons[polygonIndex].Corners[3].y); Diagonal.indexOfCorner1 = 1; Diagonal.indexOfCorner2 = 3; } startingPoint[polygonIndex] = Diagonal.indexOfCorner1; // For square A if (polygonIndex == orderOfPolygons[0]) { // Select the corner which is farther from the center of D dist = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1], Polygons[orderOfPolygons[3]].Center); dist2 = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner2], Polygons[orderOfPolygons[3]].Center); if (dist2 > dist) startingPoint[polygonIndex] = Diagonal.indexOfCorner2; } // For square D else if (polygonIndex == orderOfPolygons[3]) { // Select the corner which is closer to the center of A dist = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1], Polygons[orderOfPolygons[0]].Center); dist2 = calcDistanceBet2Points(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner2], Polygons[orderOfPolygons[0]].Center); if (dist2 < dist) startingPoint[polygonIndex] = Diagonal.indexOfCorner2; } // For remaining squares else { // Consider vector DV as the vector joining D's center to the current polygon's center. // Select the corner which is on the same side of DV as that of A's center. Vec3f DV = Vec3f(Polygons[polygonIndex].Center.x - Polygons[orderOfPolygons[3]].Center.x, Polygons[polygonIndex].Center.y - Polygons[orderOfPolygons[3]].Center.y, 0); Vec3f cP1 = DV.cross(DA3); Vec3f DCorner = Vec3f(Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1].x - Polygons[orderOfPolygons[3]].Center.x, Polygons[polygonIndex].Corners[Diagonal.indexOfCorner1].y - Polygons[orderOfPolygons[3]].Center.y, 0); Vec3f cP2 = DV.cross(DCorner); if (cP1.val[2]*cP2.val[2] < -1) startingPoint[polygonIndex] = Diagonal.indexOfCorner2; } } for (int polygonIndex = 0; polygonIndex < 6; polygonIndex++) { cornerIndex = startingPoint[orderOfPolygons[polygonIndex]]; for (int k=0; k<4; k++) { Corners[cornerLabel-1] = Polygons[orderOfPolygons[polygonIndex]].Corners[cornerIndex]; cornerLabel++; cornerIndex = (cornerIndex + 1) % 4; } } }
void VRMesure::processBar(Vec3f p1, Vec3f p2) { //p1 -= l->getWorldPosition(); //p2 -= l->getWorldPosition(); Vec3f d = p2-p1; Vec3f t1, t2; if (d[1] < d[0]) t1 = d.cross(Vec3f(0,1,0));//take the biggest axis else t1 = d.cross(Vec3f(1,0,0)); t2 = d.cross(t1); t1.normalize(); t1 *= 0.01; t2.normalize(); t2 *= 0.01; GeometryRecPtr geo = l->getMesh(); GeoPnt3fPropertyRecPtr pos = dynamic_cast<GeoPnt3fProperty*>(geo->getPositions()); GeoVec3fPropertyRecPtr norms = dynamic_cast<GeoVec3fProperty*>(geo->getNormals()); pos->setValue(p1+t1, 5); pos->setValue(p1+t1, 12); pos->setValue(p1+t1, 20); pos->setValue(p1+t2, 4); pos->setValue(p1+t2, 9); pos->setValue(p1+t2, 21); pos->setValue(p1-t1, 6); pos->setValue(p1-t1, 11); pos->setValue(p1-t1, 19); pos->setValue(p1-t2, 7); pos->setValue(p1-t2, 14); pos->setValue(p1-t2, 18); pos->setValue(p2+t1, 0); pos->setValue(p2+t1, 13); pos->setValue(p2+t1, 22); pos->setValue(p2+t2, 1); pos->setValue(p2+t2, 8); pos->setValue(p2+t2, 23); pos->setValue(p2-t1, 3); pos->setValue(p2-t1, 10); pos->setValue(p2-t1, 17); pos->setValue(p2-t2, 2); pos->setValue(p2-t2, 15); pos->setValue(p2-t2, 16); norms->setValue(p1+t1, 5); norms->setValue(p1+t1, 12); norms->setValue(p1+t1, 20); norms->setValue(p1+t2, 4); norms->setValue(p1+t2, 9); norms->setValue(p1+t2, 21); norms->setValue(p1-t1, 6); norms->setValue(p1-t1, 11); norms->setValue(p1-t1, 19); norms->setValue(p1-t2, 7); norms->setValue(p1-t2, 14); norms->setValue(p1-t2, 18); norms->setValue(p2+t1, 0); norms->setValue(p2+t1, 13); norms->setValue(p2+t1, 22); norms->setValue(p2+t2, 1); norms->setValue(p2+t2, 8); norms->setValue(p2+t2, 23); norms->setValue(p2-t1, 3); norms->setValue(p2-t1, 10); norms->setValue(p2-t1, 17); norms->setValue(p2-t2, 2); norms->setValue(p2-t2, 15); norms->setValue(p2-t2, 16); }
void labelPolygons(_Polygon (&Polygons)[6], int (&orderOfPolygons)[6]) { float distanceFromAToD = 0, dist; for (int k=0; k<6; k++) { if (k != orderOfPolygons[0]) { dist = calcDistanceBet2Points(Polygons[orderOfPolygons[0]].Center, Polygons[k].Center); if (dist > distanceFromAToD) { distanceFromAToD = dist; orderOfPolygons[3] = k; } } } Vec3f DA = Vec3f(Polygons[orderOfPolygons[0]].Center.x - Polygons[orderOfPolygons[3]].Center.x, Polygons[orderOfPolygons[0]].Center.y - Polygons[orderOfPolygons[3]].Center.y, 0); float distFromD, distBCFromD[2][2] = {0}, distEFFromD[2][2] = {0}; int indexBC = 0, indexEF = 0; for (int k=0; k<6; k++) { if (k == orderOfPolygons[3] || k == orderOfPolygons[0]) continue; Vec3f DV = Vec3f(Polygons[k].Center.x - Polygons[orderOfPolygons[3]].Center.x, Polygons[k].Center.y - Polygons[orderOfPolygons[3]].Center.y, 0); distFromD = calcDistanceBet2Points(Polygons[orderOfPolygons[3]].Center, Polygons[k].Center); Vec3f cP = DA.cross(DV); if (cP.val[2] > 0) { distEFFromD[indexEF][0] = distFromD; distEFFromD[indexEF][1] = k; indexEF++; } else { distBCFromD[indexBC][0] = distFromD; distBCFromD[indexBC][1] = k; indexBC++; } if (distBCFromD[0][0] > distBCFromD[1][0]) { orderOfPolygons[1] = distBCFromD[0][1]; // Polygon B orderOfPolygons[2] = distBCFromD[1][1]; // Polygon C } else { orderOfPolygons[1] = distBCFromD[1][1]; // Polygon B orderOfPolygons[2] = distBCFromD[0][1]; // Polygon C } if (distEFFromD[0][0] > distEFFromD[1][0]) { orderOfPolygons[5] = distEFFromD[0][1]; // Polygon F orderOfPolygons[4] = distEFFromD[1][1]; // Polygon E } else { orderOfPolygons[5] = distEFFromD[1][1]; // Polygon F orderOfPolygons[4] = distEFFromD[0][1]; // Polygon E } } }