C++ Matrix4d::inverse方法代码示例

int main(void)
{
   const int n =6; // 4I , 2 lambda
   int info;

   // system params
   Coil coil;
   Magnet magnet;
   coil.d = .0575;
   coil.R = 0.075;

   magnet.x =  0.01;
   magnet.y =  0.0;
   magnet.Fx = 1 * pow(10,-8);
   magnet.Fy = 1.* pow(10,-8);
   magnet.gamma = 35.8 * pow(10,-6);
   double th = -19 * PI/180; // convert angle in degrees to radians

   double Bmag = 10. * pow(10,-6);
   double Bx = Bmag * cos(th);
   double By = Bmag * sin(th);

   Vector2d mvec(Bx,By);
   mvec = magnet.gamma/sqrt(Bx * Bx + By * By)*mvec;

   cout << "mvec" << endl << mvec << endl;

   MatrixXd Bmat = computeBmat(magnet.x,magnet.y,coil.R,coil.d);
   cout << "Bmat" << endl << Bmat << endl;
   //MatrixXd 

   Matrix4d A;
   cout << "A" << endl << A << endl;

   A.block(0,0,2,4) = Bmat;
   cout << "A row 1-2" << endl << A << endl;
   A.block(2,0,1,4) = mvec.transpose() * Dx(magnet.x,magnet.y,coil.R,coil.d);
   A.block(3,0,1,4) = mvec.transpose() * Dy(magnet.x,magnet.y,coil.R,coil.d);
   cout << "A " << endl << A << endl;

   cout << "A inverse is:" << endl << A.inverse() << endl;

   Vector4d BF;
   BF << Bx,By,magnet.Fx,magnet.Fy;
   cout << "BF: " << endl << BF << endl;
   Vector4d Isolve = A.inverse() * BF;

   cout << "Isolve: " << endl << Isolve << endl;
   return 0;
}

void VRMolecule::rotateBond(int a, int b, float f) {
    if (atoms.count(a) == 0) return;
    if (atoms.count(b) == 0) return;
    VRAtom* A = atoms[a];
    VRAtom* B = atoms[b];

	uint now = VRGlobals::CURRENT_FRAME + rand();
    A->recFlag = now;

    Vec3d p1 = Vec3d( A->getTransformation()[3] );
    Vec3d p2 = Vec3d( B->getTransformation()[3] );
    Vec3d dir = p2-p1;
    Quaterniond q(dir, f);
    Matrix4d R;
    R.setRotate(q);

    Matrix4d T;
    T[3] = B->getTransformation()[3];
    Matrix4d _T;
    T.inverse(_T);
    T.mult(R);
    T.mult(_T);

    //cout << "ROTATE bound " << a << "-" << b << " around " << dir << " with " << f << endl;

    B->propagateTransformation(T, now);
    updateGeo();
}

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

    lhs = _getLhs(_totalLength);

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

    bool calcLegAngles(double** out_q) {

        rpy = rpyFromRot(R);

        cy = cos(rpy[2]);
        sy = sin(rpy[2]);
        cp = cos(rpy[1]);
        sp = sin(rpy[1]);
        cr = cos(rpy[0]);
        sr = sin(rpy[0]);

        Vector3 O1(0.0, sign * robot->l0, -robot->l1);
        const Vector3& F = p;
        Vector3 V1 = F - O1;
        Vector3 XF(cy * cp, sy * cp, -sp);
        Vector3 V1xXF = V1.cross(XF);
        Vector3 Z2 = -sign * V1xXF / V1xXF.norm();

        q[0] = atan2(-sign * Z2[0], sign * Z2[1]);
        q[1] = asin(-sign * Z2[2]);

        double c1 = cos(q[0]);
        double s1 = sin(q[0]);
        double c2 = cos(q[1]);
        double s2 = sin(q[1]);
        double s1s2 = s1 * s2;
        double c1s2 = c1 * s2;
        slo1 = sign * robot->lo1;

        TB2 <<
            s1s2, -sign * c1, -sign * s1 * c2,  slo1 * s1 + robot->l2 * c1 + robot->lo2 * s1s2,
            -c1s2, -sign * s1,  sign * c1 * c2,  sign * robot->l0 - slo1 * c1 + robot->l2 * s1 - robot->lo2 * c1s2,
            -c2,    0.0,      -sign * s2,      -robot->l1 - robot->lo2 * c2,
            0.0,   0.0,       0.0,            1.0;

        Vector3 V2 = (TB2.inverse() * Vector4d(F[0], F[1], F[2], 1.0)).head(3);
        double D = (V2.squaredNorm() - robot->l3 * robot->l3 - robot->l4 * robot->l4) / (2.0 * robot->l3 * robot->l4);

        if(fabs(D) > 1.0){
            return false;
        }

        q[3] = atan2(-sign * sqrt(1.0 - D * D), D);
        double c4 = cos(q[3]);
        double s4 = sin(q[3]);
        
        double beta = atan2(-V2[1], sqrt(V2[0] * V2[0] + V2[2] * V2[2]));
        double alpha = atan2(robot->l4 * s4, robot->l3 + robot->l4 * c4);
        q[2] = -(beta - alpha);
        
        q[3] = -q[3];
        
        double c3 = cos(q[2]);
        double s3 = sin(q[2]);
        double q2q3 = q[2] + q[3];
        double c34 = cos(q2q3);
        double s34 = sin(q2q3);
        
        Matrix4d T24;
        T24 <<
            c34,  s34,  0, robot->l3 * c3 + robot->l4 * c34,
            s34, -c34,  0, robot->l3 * s3 + robot->l4 * s34,
            0.0,  0.0, -1, 0.0,
            0.0,  0.0,  0, 1.0;
        
        TB4.noalias() = TB2 * T24;

        double spsr = sp * sr;
        double spcr = sp * cr;
    
        TBF <<
            cy * cp, -sy * cr + cy * spsr,  sy * sr + cy * spcr, p.x(),
            sy * cp,  cy * cr + sy * spsr, -cy * sr + sy * spcr, p.y(),
            -sp,       cp * sr,              cp * cr,             p.z(),
            0,        0,                    0,                   1.0;
        
        Matrix4d T4F;
        T4F.noalias() = TB4.inverse() * TBF;

        q[4] = atan2(-sign * T4F(0,0),  sign * T4F(1,0));
        q[5] = atan2( sign * T4F(2,2), -sign * T4F(2,1));

        // Numerical refining
        
        TB0 <<
            0.0, -1.0, 0.0, 0.0,
            1.0,  0.0, 0.0, sign * robot->l0,
            0.0,  0.0, 1.0, 0.0,
            0.0,  0.0, 0.0, 1.0;
        
        Af <<
            0.0, 1.0, 0.0, 0.0,
            -1.0, 0.0, 0.0, 0.0,
            0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 1.0;

        bool solved = false;

        int i;
        for(i=0; i < 30; ++i){
//.........这里部分代码省略.........

// Given the solution of the equation up to the time step solution.timestep - 1, computes the solution at the sime step solution.timestep and increases timestep by one
void step(double * data, Solution & solution, const SimulationContext & context, const vector<shared_ptr<SimulationResult>> & results)
{
    // The equation is result = (2-Re dt)^(-1) (2 + Re dt) se + (2-Re dt)^(-1) G s

    // Total Spin
    Vector4d s = 0.5*(3.0*solution.current.total.spin - solution.previous.total.spin);

    // Aliases
    const Settings & settings = context.settings();
    double dt = data[Index::Duration] / data[Index::TimestepCount];

    data[Index::Timestep]   = solution.timeStep;
    data[Index::Time]       = solution.timeStep * data[Index::Duration] / data[Index::TimestepCount];

    context.evaluateFunctions(ComputeFor::Step, Compute::OnceForEachStep, data);

    // test
    double h = accumulate(solution.current.spin.begin(), solution.current.spin.end(), 0.0, [](double old, const Vector4d & s) {return old + abs(s(2));}) / solution.current.spin.size();
    (void)h;

    // Solving the equation of motion for the individual spins
    for(unsigned int i = 0; i < settings.energySamples().size(); ++i) {
        Vector4d & spin = solution.current.spin.at(i);

        data[Index::EnergyIndex] = i;
        data[Index::Energy] = settings.energySamples().at(i);

        context.evaluateFunctions(ComputeFor::Step, Compute::OnceForEachEnergy, data);

        // Matrix initialization
        Matrix4d R, RD;
        // R = RF + RE + RD + RL;

        // Pseudo magnetic field
        /* 0,                      -data[Index::Frequency],    0, 0,
         * data[Index::Frequency],  0,                         0, 0,
         * 0,                       0,                         0, 0,
         * 0,                       0,                         0, 0;
         */

        R.setZero();
        R(0,1) += -data[Index::Frequency];
        R(1,0) += +data[Index::Frequency];

        // Exchange
        /*  0,                                 -data[Index::Exchange]*s(2)*s(3),    data[Index::Exchange]*s(1)*s(3),   0,
         *  data[Index::Exchange]*s(2)*s(3),    0,                                 -data[Index::Exchange]*s(0)*s(3),   0,
         * -data[Index::Exchange]*s(1)*s(3),    data[Index::Exchange]*s(0)*s(3),    0,                                 0,
         *  0,                                  0,                                  0,                                 0;
         */
        R(0,1) += -data[Index::Exchange]*s(2)*s(3);
        R(0,2) += +data[Index::Exchange]*s(1)*s(3);
        R(1,0) += +data[Index::Exchange]*s(2)*s(3);
        R(1,2) += -data[Index::Exchange]*s(0)*s(3);
        R(2,0) += -data[Index::Exchange]*s(1)*s(3);
        R(2,1) += +data[Index::Exchange]*s(0)*s(3);

        // Damping
        /* -data[Index::Damping],  0,                       0,                     0,
         *  0,                    -data[Index::Damping],    0,                     0,
         *  0,                     0,                      -data[Index::Damping],  0,
         *  0,                     0,                       0,                     0;
         */
        RD.setZero();
        RD(0,0) += -data[Index::Damping];
        RD(1,1) += -data[Index::Damping];
        RD(2,2) += -data[Index::Damping];

        R += RD;

        // Atom losses
        double dfls = 0.25 * (data[Index::AtomLossFromF1] + data[Index::AtomLossFromF2]);    // Total atom losses to spectator states
        double dfld = 0.25 * (data[Index::AtomLossFromF1] - data[Index::AtomLossFromF2]);    // Difference of atom losses to spectator states
        /* 0, 0,  0,                        0,
         * 0, 0,  0,                        0,
         * 0, 0, -(dfls*s(3) + dfld*s(2)), -(dfld*s(3) + dfls*s(2)),
         * 0, 0, -(dfld*s(3) + dfls*s(2)), -(dfls*s(3) + dfld*s(2));*/
        R(2,2) += -(dfls*s(3) + dfld*s(2));
        R(2,3) += -(dfld*s(3) + dfls*s(2));
        R(3,2) += -(dfld*s(3) + dfls*s(2));
        R(3,3) += -(dfls*s(3) + dfld*s(2));

        // Computation

        Matrix4d RT; // = 2 + R * dt
        Matrix4d RI; // = (2 - R * dt).inverse();

        // We can't inverse in place, so we use RT as a temporary to compute RI
        RT = 2.0 * Matrix4d::Identity() - dt * R;
        RI = RT.inverse();
        RT = 2.0 * Matrix4d::Identity() + dt * R;

        spin = RI * (RT * spin - dt * RD * s).eval();
    }

    // Update values at the previous timestep
    solution.previous.total.spin = solution.current.total.spin;
    solution.previous.total.phase = solution.current.total.phase;

//.........这里部分代码省略.........

void BoneActor::doIK(Vector3f loc, bool bAbsolute){

    /*********************************************************************************

        3 part process:

        1. gather all necessary numbers
        2. rotate the system into target plane (plane between target point and shoulder)
        3. do 2D IK

    *********************************************************************************/


    //leave if we don't have two parents
    if (!base || !base->base ){
        cout << "no double-base!!! Cannot do IK" << endl;
        Actor::setLocation(loc);
        return;
    }

    //for debug stuff
    f+=0.01;

    ///clear transform Matrices!

    Matrix4d initialTransform;

    initialTransform=base->base->transformMatrix;

    base->base->transformMatrix.identity();
    base->transformMatrix.identity();
    transformMatrix.identity();
    base->base->baseMatrix=calcMatrix(base->base);
    base->baseMatrix=calcMatrix(base);
    baseMatrix=calcMatrix(this);

    ///gathering numbers

    Vector3f currentUpperVec, currentLowerVec, initialUpperVec, initialLowerVec;

    MeshData* myMesh=renderer->vboList[parent->vboMeshID];
    //find original angles for non-aligned bones - from mesh data!
    for (int i=0;i<(int)myMesh->bones.size();i++){
        if (myMesh->bones[i]->name==base->name)
            initialUpperVec=myMesh->bones[i]->boneMatrix->getTranslation();
        if (myMesh->bones[i]->name==name)
            initialLowerVec=myMesh->bones[i]->boneMatrix->getTranslation();
    }

    float upperLength = initialUpperVec.length();
    float lowerLength = initialLowerVec.length();

    //initial Full - without any transformations
    //this is for the relative positions of the bones to each other
    Vector3f initialFull= initialUpperVec + initialLowerVec;
    //project upper on full vector to get upper Angle

    initialUpperVec.normalize();
    initialLowerVec.normalize();
    initialFull.normalize();

    Vector3f currentFull=base->base->baseMatrix.getTranslation() - baseMatrix.getTranslation();
    Vector3f targetFull=base->base->baseMatrix.getTranslation() - loc;

    float targetFullLength = targetFull.length();


    ///out of bounds!!!
    if (targetFullLength>=upperLength+lowerLength){
        cout << "IK on " << name << " is out of bounds!" << endl;
        cout << targetFullLength << endl;
        return;
    }

    currentFull.normalize();
    targetFull.normalize();

    ///rotating into IK plane
    Matrix4f parentMatrix=parent->baseMatrix;
    parentMatrix.setTranslation(Vector3f(0,0,0));

    Vector3f rotAxis = currentFull.crossProduct(targetFull);
    rotAxis=parentMatrix.inverse() * rotAxis;
    rotAxis.normalize();

    float rot= acos(currentFull.dotProduct(targetFull));

    //check if they're on each other!
    if (rot!=rot){
        cout << "already in plane!" << endl;
        rot=0.0;    //no rotation in that case!
    }
    Vector3f xA,yA,zA;
    yA=rotAxis;
    xA=currentFull;
    zA=yA.crossProduct(xA);

    zA.normalize();

    Matrix4f rotMatrix,angleMatrix;
//.........这里部分代码省略.........

void Surface::trans(Matrix4d& m, Matrix4d& inv) {
	_trans = true;
	_mInv = inv;
	_mTrans = m.inverse().transpose();
	_b = _b.transform(m);
}