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