C++ Matrix4d类代码示例

void Thread_RRT::getNextGoal(RRT_Node& goal, RRT_Node& goalThisIter)
{
  //first, see if we are sampling randomly, or around goal
  double randNum = ((double)rand()) / ((double)RAND_MAX);
  if (randNum < RANDOM_SAMPLE_THRESH)
  {
    //goalThisIter.copyNode(goal);   
    //return;
    
    setThisGoalNearGoal(goal, goalThisIter);
    return;



  }

  //sample random goal
  Matrix4d startTrans;
  goal.thread->getStartTransform(startTrans);

  Vector3d startPos = startTrans.corner(Eigen::TopRight,3,1);
  Vector3d startTan = startTrans.corner(Eigen::TopLeft,3,1);
  goalThisIter.thread = new Thread(goal.thread->length(), startPos, startTan);

  goalThisIter.thread->minimize_energy_fixedPieces();
  goalThisIter.setPoints();

}

//////////////////////////////////////////////////////////////////////////
// Get look vector (this is NOT a rotation!)
Vector3d cEntity::GetLookVector(void) const
{
	Matrix4d m;
	m.Init(Vector3d(), 0, m_Rot.x, -m_Rot.y);
	Vector3d Look = m.Transform(Vector3d(0, 0, 1));
	return Look;
}

void AABB::draw_mesh()
{
    glColor3d(color[0],color[1],color[2]);
    Vector3d n;

    glPushMatrix();

    glTranslated(-center[0], -center[1], -center[2]);
    Matrix4d R(q_now);
    Matrix4d RT = R.transpose();
    glMultMatrixd(RT.ptr());
    glScaled(zoom_val,zoom_val,zoom_val);

    glBegin(GL_TRIANGLES);
    for(unsigned int j=0;j<p.size();++j) {
        glVertex3dv(p[j].ptr());
    }
    glEnd();
    glPopMatrix();

    glColor3d(color[0],color[1],color[2]);
    Vector3d u=q_now*p[0]*zoom_val-center,
             v=q_now*p[1]*zoom_val-center,
             w=q_now*p[2]*zoom_val-center;

    glBegin(GL_TRIANGLES);
    glVertex3dv(u.ptr());
    glVertex3dv(v.ptr());
    glVertex3dv(w.ptr());
    glEnd();
}

 void TrfmTranslate::applyDeriv(const Dof* q, Matrix4d& m){
     for(unsigned int i=0; i<mDofs.size(); i++){
         if(mDofs[i] != q) m.row(i).setZero();
         else m.row(i) = m.row(3);
     }
     m.row(3).setZero();
 }

void igl::sort_triangles(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedI> & I)
{
  using namespace Eigen;
  using namespace igl;
  using namespace std;
  // Put model, projection, and viewport matrices into double arrays
  Matrix4d MV;
  Matrix4d P;
  glGetDoublev(GL_MODELVIEW_MATRIX,  MV.data());
  glGetDoublev(GL_PROJECTION_MATRIX, P.data());
  if(V.cols() == 3)
  {
    Matrix<typename DerivedV::Scalar, DerivedV::RowsAtCompileTime,4> hV;
    hV.resize(V.rows(),4);
    hV.block(0,0,V.rows(),V.cols()) = V;
    hV.col(3).setConstant(1);
    return sort_triangles(hV,F,MV,P,FF,I);
  }else
  {
    return sort_triangles(V,F,MV,P,FF,I);
  }
}

void VRAtom::computePositions() {
    computeGeo();

    string g = geo;
    if (AtomicStructures.count(geo) == 0) { cout << "Error: " << geo << " is invalid!\n"; return; }

    vector<Matrix4d> structure = AtomicStructures[geo];
    for (auto& b : bonds) {
        if (b.first >= (int)structure.size()) break;
        if (b.second.extra) continue;

        Matrix4d T = transformation;
        Matrix4d S = structure[b.first];

        VRAtom* a = b.second.atom2;
        if (a == 0) { // duplets
            float r = 0.5*b.second.atom1->getParams().radius;
            S[3] *= r; S[3][3] = 1;
            T.mult(S);
            T.mult(Pnt3d(1,0,0)*r, b.second.p1);
            T.mult(Pnt3d(-1,-0,0)*r, b.second.p2);
            continue;
        }

        if (a->ID <= ID) continue;
        T.mult(S);
        a->transformation = T;
    }
}

void Thread::printThreadInfo()
{
  //print out some information
	Eigen::Transform3d first_rotation_transform(Eigen::AngleAxisd(_angle_first_rot, _tangents[0]));
  int numPieces = 0;
  double energy = 0.0;
	double twist = 0.0;
  double length = 0.0;
  ThreadPiece* currPiece = threadList;
	Matrix4d trans = _translate_to_start*first_rotation_transform*_transform_to_start;
  trans.corner(Eigen::TopLeft,3,3) *= _length;
  while (currPiece != NULL)
  {
    numPieces++;
    energy += currPiece->_length*(currPiece->_torsion*currPiece->_torsion + currPiece->_curvature*currPiece->_curvature); 
		//twist += currPiece->_length*currPiece->_torsion;
    length += currPiece->_length;
    std::cout << "length:" << currPiece->_length << std::endl;
    std::cout << "curvature: " << currPiece->_curvature/_length << "  torsion: " << currPiece->_torsion/_length << std::endl;
    trans *= currPiece->_transform;
    currPiece = currPiece->_next_segment;
  }

  std::cout << "Optimal thread has " << numPieces << " pieces, and a calculated energy of " << energy*_length << std::endl;
  std::cout << "Final Position: \n" << trans.corner(Eigen::TopRight, 3,1) << std::endl;
  std::cout << "Final Position Wanted: \n" << _positions[1] << std::endl;
  std::cout << "Final Tangent: \n" << trans.corner(Eigen::TopLeft, 3, 1)/_length << std::endl;
}

Pose getDirTransform(Pose transformation) {
    Matrix4d dirTransformation = transformation.asMatrix();
    dirTransformation.invert();
    dirTransformation.transpose();
    Pose dirTransform(dirTransformation);
    return dirTransform;
}

Matrix6d adjoint_se3(Matrix4d T){
    Matrix6d AdjT = Matrix6d::Zero();
    Matrix3d R = T.block(0,0,3,3);
    AdjT.block(0,0,3,3) = R;
    AdjT.block(0,3,3,3) = skew( T.block(0,3,3,1) ) * R ;
    AdjT.block(3,3,3,3) = R;
    return AdjT;
}

 Matrix4d rotateZ(double _angle)
 {
 	Matrix4d m = Matrix4d::Identity();
 	Matrix3d r;
 	r = AngleAxisd(_angle, Vector3d::UnitZ());
 	m.corner(TopLeft, 3, 3) = r;
 	return m;
 }

Ray Camera::GenerateRay(const Vec2f& uv, const Vec2f& auv, float time) {
    Ray ray = lens->GenerateRay(uv, auv, time);
    if(!xform->IsIdentity()) {
        Matrix4d m = xform->GetTransform(time);
        ray = Ray(m.TransformPoint(ray.E), m.TransformVector(ray.D).GetNormalized(), ray.time);
    }
    return ray;
}

Range3f RayTesselatedSpherePrimitive::ComputeBoundingBox(const Intervalf& time, int approximationSamples) {
    Vec3f p;
    float r;
    Matrix4d m;
    Matrix4d mi;
    _ResolveSphereAttrib(p,r,m,mi);
    return m.TransformBBox(ElementOperations::SphereBoundingBox(p,r));
}

//==== Get Total Transformation Matrix from Original Points ====//
Matrix4d PtCloudGeom::GetTotalTransMat()
{
    Matrix4d retMat;
    retMat.initMat( m_ScaleMatrix.data() );
    retMat.postMult( m_ModelMatrix.data() );

    return retMat;
}

Matrix4d virtuose::getPose(float f[7]) {
    Matrix4d m;
    Vec3d pos(f[1], f[2], f[0]);
    Quaterniond q;
    q.setValue(f[4], f[5], f[3]);
    m.setRotate(q);
    m.setTranslate(pos);
    return m;
}

ClothoidPtr ClothoidFitter::getCurve() const
{
    Matrix4d lhs;

    lhs = _getLhs(_totalLength);

    Vector4d abcd = lhs.inverse() * _rhs;
    return getClothoidWithParams(abcd);
}