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

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