/***************************************************************
 * PrLink.cpp
 *
 * This implements a robot link class.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 03/19/98: K.C. Chang: added homeT_.
 * 11/07/97: K.C. Chang: created.
 */

#include "PrLink.h"
#include <assert.h>
#include <stdio.h>

// ddqi = (taui - si^T Iai (i-1Xi)^T Ai-1 - si^T(Zai + Iai Ci))/(si^T Iai si)
Float PrLink::GetJointAcceleration()
{
    assert(parent_ != NULL);

    PrVector6 &XtAp = tmpV61;

    Xform6T(parent_->A_,XtAp);
    Float St_Ia_Xt_Ap = Ia_S_.dot(XtAp);

    return (tau_ - St_Ia_Xt_Ap - St_Za_plus_Ia_C_)*inv_St_Ia_S_;
}

Float PrLink::jointAcceleration()
{
    Sddq_[jointAxis_] = GetJointAcceleration();
    Acceleration();

    return Sddq_[jointAxis_];
}

PrLink::PrLink(int jointType,int jointAxis)
: jointType_(jointType), jointAxis_(jointAxis)
{
    parent_  = child_ = NULL;
    inertiaMatrixFlag_ = PR_LINK_ERROR;
    if (jointType_ != PR_LINK_JOINT_REVOLUTE 
	&& jointType_ != PR_LINK_JOINT_PRISMATIC)
      {
	  printf ("Error: PrLink: Unknown joint type(%d)\n",
		  jointType_);
      }
}

void PrLink::centerOfMass(const PrVector3 &com)
{
    if (jointType_ == PR_LINK_JOINT_PRISMATIC)
      prismaticCOM_ = com[jointAxis_];
    COM_ = com;
}

void PrLink::inertia(Float Ixx,Float Iyy,Float Izz)
{
    Inertia_.values(Ixx,Iyy,Izz);
}

void PrLink::homePosition(Float alpha, Float a, Float d, Float theta)
{
/*
    sinAlpha_ = sin(alpha);
    cosAlpha_ = cos(alpha);
    sinAlpha_2_ = sin(alpha*0.5);
    cosAlpha_2_ = cos(alpha*0.5);
*/

    if (jointType_ == PR_LINK_JOINT_REVOLUTE)
      theta = 0.0;
    else if (jointType_ == PR_LINK_JOINT_PRISMATIC)
      d = 0.0;
    homeT_.rotation().values(cos(alpha*0.5),sin(alpha*0.5),0.0,0.0);
    homeT_.translation().values(a,0.0,0.0);
    UpdateLocalRotation(theta);
    UpdateLocalTranslation(d);

    UpdateGlobalTransformation();
}

void PrLink::homePosition(const PrTransform &home, Float d, Float theta)
{
    if (jointType_ == PR_LINK_JOINT_REVOLUTE)
      theta = 0.0;
    else if (jointType_ == PR_LINK_JOINT_PRISMATIC)
      d = 0.0;
    homeT_ = home;
    UpdateLocalRotation(theta);
    UpdateLocalTranslation(d);

    UpdateGlobalTransformation();
}

void PrLink::invDynamics(Float ddq)
{
    Sddq_[jointAxis_] = ddq;
    Acceleration();
    NetForce();
    F_ = Fnet_;
}

void PrLink::fwdDynamics(Float tau)
{
    tau_ = tau;
    ArticulatedInertiaMatrix();
    ArticulatedBiasForce();
}

// Li = Ii + [Lh - LhS (StLhS)inv StLh]
// Lh = iXh Lh iXh^T
void PrLink::articulatedOSIM()
{
  assert(parent_ != NULL);

  if (jointType_ == PR_LINK_JOINT_PRISMATIC
      || inertiaMatrixFlag_ == PR_LINK_ERROR)
    initInertiaMatrix();
  L_ = I_;

  PrMatrix6 &Lh = tmpM61;
  PrVector6 &LhS = tmpV61;
  PrMatrix6 &LhSStLh = tmpM62;
  PrMatrix6 &LhSStLh_div_StLhS = tmpM62;
  PrMatrix6 &Lh_minus_LhSStLh_div_StLhS = tmpM62;

  similarityXform6inv(parent_->L_,Lh);

  LhS[0].values(Lh[jointType_][0][jointAxis_],
		Lh[jointType_][1][jointAxis_],
		Lh[jointType_][2][jointAxis_]);
  LhS[1].values(Lh[jointType_+2][0][jointAxis_],
		Lh[jointType_+2][1][jointAxis_],
		Lh[jointType_+2][2][jointAxis_]);
      
  LhS.multiplyTransposed(LhS,LhSStLh);
  Float inv_StLhS = 1.0/Lh[3*jointType_][jointAxis_][jointAxis_];
  LhSStLh.multiply(inv_StLhS,LhSStLh_div_StLhS);
  Lh.subtract(LhSStLh_div_StLhS,Lh_minus_LhSStLh_div_StLhS);
  L_ += Lh_minus_LhSStLh_div_StLhS;
}

void PrLink::state(Float q,Float dq,int dynamicsType)
{
    Sdq_[jointAxis_] = dq;
    if (jointType_ == PR_LINK_JOINT_PRISMATIC)
      {
	  COM_[jointAxis_] = prismaticCOM_ + q;
      }
    if (dynamicsType == PR_LINK_FWD_DYNAMICS)
      {
	  if (jointType_ == PR_LINK_JOINT_PRISMATIC
	      || inertiaMatrixFlag_ == PR_LINK_ERROR)
	    initInertiaMatrix();
      }

    Position(q);
    Velocity();
    CoriolisCentrifugal();
    BiasForce();
    if (dynamicsType == PR_LINK_FWD_DYNAMICS)
      InitFwdDynamics();
}

Float PrLink::jointForce()
{ 
    TotalForce(); 

    return  F_[jointType_][jointAxis_] + Friction();
}

// J = [ Z x (Pon - Pin); Z]
PrVector6 &PrLink::globalJacobian(const PrVector3 &globalP)
{
    globalT_.rotation().column(jointAxis_,J_[jointType_]);
    if (jointType_ == PR_LINK_JOINT_REVOLUTE)
      {
	  assert(&globalP != NULL);
	  J_[1].cross(globalP - globalT_.translation(),J_[0]);
      }
    return J_;
}

void PrLink::display()
{
    printf("link(id=%d):\n",id_);
/*
    localT_.display("local T");
    globalT_.display("global T");
*/
/*
    COM_.display("center of mass");
    printf("\n");
    Inertia_.display("inertia");
*/
    V_.display("local V");
    C_.display("local C");
    A_.display("local A");
    Z_.display("local Z");

    Fnet_.display("local Fnet");
    F_.display("local F");
    I_.display("local I");

    printf("local x=\n");
    R_.display("M6(0,0)");
    (Px_*R_).display("M6(1,0)");
    Ia_.display("local Ia");
    Za_.display("local Za");
}

// private methods

// translation = (alpha, -sin(alpha) * d, cos(alpha) * d)
// alpha is a constant
/*
void PrLink::UpdateLocalTranslation(Float q)
{
    localT_.translation()[1] = -sinAlpha_ * q;
    localT_.translation()[2] = cosAlpha_ * q;
    localT_.translation().cross(Px_);
}
*/
void PrLink::UpdateLocalTranslation(Float q)
{
    PrVector3 trans(R_[0][jointAxis_]*q,R_[1][jointAxis_]*q,R_[2][jointAxis_]*q);
    homeT_.translation().add(trans,localT_.translation());
    localT_.translation().cross(Px_);
}

// R = R_home * R_theta 
//   = Rx(alpha) * Rz(theta)
// q = q_home * q_theta
//   = q(x_axis,alpha) * q(z_axis,theta)
//   = [cos(alpha/2),(sin(alpha/2),0,0)] * [cos(theta/2),(0,0,sin(theta/2))]
//   = [c(a/2)c(q/2), (s(a/2)c(q/2),-s(a/2)s(q/2),c(a/2)s(q/2))]
/*
void PrLink::UpdateLocalRotation(Float q)
{
    Float cosQ_2 = cos(q*0.5);
    Float sinQ_2 = sin(q*0.5);
    localT_.rotation().values(cosAlpha_2_ * cosQ_2,
			      sinAlpha_2_ * cosQ_2,
			      -sinAlpha_2_ * sinQ_2,
			      cosAlpha_2_ * sinQ_2);
    localT_.rotation().matrix(R_);
    R_.transpose(Rt_);
}
*/
void PrLink::UpdateLocalRotation(Float q)
{
	PrVector3 v(0.0,0.0,0.0);
	v[jointAxis_] = sin(q*0.5);
    PrQuaternion rot(cos(q*0.5),v);
    homeT_.rotation().multiply(rot,localT_.rotation());
    localT_.rotation().matrix(R_);
    R_.transpose(Rt_);
}

void PrLink::UpdateGlobalTransformation()
{
    if (parent_ != NULL)
      {
	  parent_->globalT_.multiply(localT_,globalT_);
      }
    else
      {
	  globalT_ = localT_;
      }
}

void PrLink::Position(Float q)
{
    if (jointType_ == PR_LINK_JOINT_REVOLUTE)
      {
	  UpdateLocalRotation(q);
      }
    else if (jointType_ == PR_LINK_JOINT_PRISMATIC)
      {
	  UpdateLocalTranslation(q);
      }

    UpdateGlobalTransformation();
}


// Vi = [vi , wi] = iXi-1 Vi-1 + Si dqi
void PrLink::Velocity()
{
    assert(parent_ != NULL);
    // Vi = iXi-1 Vi-1
    Xform6T(parent_->V_,V_);

    // Vi += Si dqi;
    V_[jointType_][jointAxis_] += Sdq_[jointAxis_];

}

// Ci = [ wi-1 x (wi-1 x i-1Pi); wi-1 x Zdq]  // revolute
// Ci = [ wi-1 x (wi-1 x i-1Pi) + 2wi-1 x Zdq; 0]  // prismatic
void PrLink::CoriolisCentrifugal()
{
    PrVector3 &Wp = tmpV31;
    PrVector3 &Pp = tmpV32;
    PrVector3 &WpxPp = tmpV33;
    PrVector3 &WpxZdq = tmpV33;

    Rt_.multiply(parent_->V_[1],Wp);
    Rt_.multiply(localT_.translation(),Pp);
    Wp.cross(Pp,WpxPp);
    Wp.cross(WpxPp,C_[0]);

    if (jointType_ == PR_LINK_JOINT_REVOLUTE)
      {
	  Wp.cross(Sdq_,C_[1]);
      }
    else if (jointType_ == PR_LINK_JOINT_PRISMATIC)
      {
	  Wp.cross(Sdq_,WpxZdq);
	  WpxZdq += WpxZdq;
	  C_[0] += WpxZdq;
      }
}

// Ai = [dvi , dwi] = iXi-1 Ai-1 + Si ddqi + Ci
void PrLink::Acceleration()
{
    assert(parent_ != NULL);

    // Ai = iXi-1 Ai-1 
    Xform6T(parent_->A_,A_);

    // Ai += Ci
    A_ += C_;

    // Ai += Si ddqi
    A_[jointType_][jointAxis_] += Sddq_[jointAxis_];
}

// Zi = [Mi(wi x (wi x di)); wi x (Ic - Mi(di x)(di x)) wi]
void PrLink::BiasForce()
{
    PrVector3 &Wxd = tmpV31;
    PrVector3 &dxW = tmpV31;
    PrVector3 &dxdxW = tmpV32;
    PrVector3 &m_times_dxdxW = dxdxW;
    PrVector3 &IcW = tmpV31;
    PrVector3 &IcW_minus_m_times_dxdxW = IcW;

    V_[1].cross(COM_,Wxd);
    V_[1].cross(Wxd,Z_[0]);
    Z_[0] *= mass_;
    
    COM_.cross(V_[1],dxW);
    COM_.cross(dxW,dxdxW);
    dxdxW *= mass_;
    Inertia_.multiply(V_[1],IcW);
    IcW -= m_times_dxdxW;
    V_[1].cross(IcW_minus_m_times_dxdxW,Z_[1]);
}

// Ii = [M Ht; H I] 
// H = Mi (di x)
// I = Iprincipal - Mi (di x)(di x) : parallel axis theorem
// Ii = [Mi -Mi(di x); Mi(di x) (Ip - Mi(di x)(di x))]
// Fneti = Ii Ai + [wi x (wi x di); wi x I wi] = Ii Ai + Zi
void PrLink::NetForce()
{
    PrVector3 &dxAlpha = tmpV31;
    PrVector3 &dxdxAlpha = tmpV32;
    PrVector3 &m_times_dxdxAlpha = dxdxAlpha;
    PrVector3 &dxA = tmpV31;
    PrVector3 &m_times_dxA = dxA;

    // Fneti = Ii Ai
    COM_.cross(A_[1],dxAlpha);
    A_[0].subtract(dxAlpha,Fnet_[0]);
    Fnet_[0] *= mass_;

    COM_.cross(dxAlpha,dxdxAlpha);
    dxdxAlpha *= mass_;
    
    Inertia_.multiply(A_[1],Fnet_[1]);
    Fnet_[1] -= m_times_dxdxAlpha;

    COM_.cross(A_[0],dxA);
    dxA *= mass_;
    Fnet_[1] += m_times_dxA;

    Fnet_ += Z_;
}

// fi = iXi+1 fi+1 + Fi
// iXi+1 = (i+1Xi)^T
//       =
// i+1Xi = [i+1Ri -i+1Ri(iPi+1 x); 0 i+1Ri]
// iXi+1 = (i+1Xi)^T = [iRi+1 0;(iPi+1 x)iRi+1 iRi+1]
void PrLink::TotalForce()
{
    // Fi-1 = Fnet i-1 + i-1Xi Fi
    assert(parent_ != NULL);
	
    if (parent_->parent_ != NULL)
	{
	    Xform6(F_,tmpV61);
	    parent_->F_ += tmpV61;
	}
/*
    // Fi = iXi+1 Fi+1
    assert(child_ != NULL);

    child_->Xform6(child_->F_,F_);

    // Fi += Fneti
    F_ += Fnet_;
*/
}

Float PrLink::Friction()
{
    Float f = friction_[0]*Sdq_[jointAxis_];  // viscous friction
    
    if (Sdq_[jointAxis_] > 0)
      f += friction_[1];	// dynamic coulomb friction
    else if (Sdq_[jointAxis_] < 0)
      f -= friction_[1];
    /*
       else
       f += friction_[2];	// static coulomb friction
       */
    return f;
}


// Iai-1 = Ii-1 + i-1Xi (Iai - (Iai si si^t Iai)/(si^t Iai si)) (i-1Xi)^T
void PrLink::ArticulatedInertiaMatrix()
{
    assert(parent_ != NULL);

    inv_St_Ia_S_ = 1.0/Ia_[3*jointType_][jointAxis_][jointAxis_];

    Ia_S_[0].values(Ia_[jointType_][0][jointAxis_],
		    Ia_[jointType_][1][jointAxis_],
		    Ia_[jointType_][2][jointAxis_]);
    Ia_S_[1].values(Ia_[jointType_+2][0][jointAxis_],
		    Ia_[jointType_+2][1][jointAxis_],
		    Ia_[jointType_+2][2][jointAxis_]);
    if (parent_->parent_ != NULL)
      {
	  PrMatrix6 &IaSStIa = tmpM61;
	  PrMatrix6 &IaSStIa_div_StIaS = tmpM61;
	  PrMatrix6 &Ia_minus_IaSStIa_div_StIaS = tmpM61;
	  PrMatrix6 &XIXt = tmpM62;

	  Ia_S_.multiplyTransposed(Ia_S_,IaSStIa);
	  IaSStIa.multiply(inv_St_Ia_S_,IaSStIa_div_StIaS);

	  /*
	  PrVector6 &StIa_div_StIaS = tmpV61;
	  Ia_S_.multiply(inv_St_Ia_S_,StIa_div_StIaS);
	  Ia_S_.multiplyTransposed(StIa_div_StIaS,IaSStIa_div_StIaS);
	  */

	  Ia_.subtract(IaSStIa_div_StIaS,Ia_minus_IaSStIa_div_StIaS);

	  similarityXform6(Ia_minus_IaSStIa_div_StIaS,XIXt);
	  parent_->Ia_ += XIXt;

      }
}

// Zai-1 = Zi-1 + i-1Xi(Zai + Iai Ci 
//                      + Iai Sai (taui - si^t (Zai + Iai Ci))/(si^t Iai si))
void PrLink::ArticulatedBiasForce()
{
    assert(parent_ != NULL);

    PrVector6 &IaC = tmpV61;
    PrVector6 &Za_plus_IaC = tmpV61;
    PrVector6 &IaS_times_Q = tmpV62;
    PrVector6 &Z = tmpV62;
    PrVector6 &XZ = tmpV61;

    Ia_.multiply(C_,IaC);
    Za_.add(IaC,Za_plus_IaC);

    St_Za_plus_Ia_C_ = Za_plus_IaC[jointType_][jointAxis_];

    if (parent_->parent_ != NULL)
      {
	  Ia_S_.multiply((tau_- St_Za_plus_Ia_C_)*inv_St_Ia_S_,IaS_times_Q);
	  Za_plus_IaC.add(IaS_times_Q,Z);
	  Xform6(Z,XZ);
	  parent_->Za_ += XZ;
      }
}

void PrLink::initInertiaMatrix()
{
    PrMatrix3 &dx = tmpM31;
    PrMatrix3 &dxdx = tmpM32;
    PrMatrix3 &m_times_dxdx = dxdx;

    I_[0].diagonal(mass_,mass_,mass_); 
    COM_.cross().multiply(mass_,I_[2]);
    I_[2].transpose(I_[1]);

    I_[3].diagonal(Inertia_[0],Inertia_[1],Inertia_[2]);

    COM_.cross(dx);
    dx.multiply(dx,dxdx);
    dxdx *= mass_;
    I_[3] -= m_times_dxdx;
    inertiaMatrixFlag_ = PR_LINK_OK;
}

void PrLink::InitFwdDynamics()
{
    Ia_ = I_;
    Za_ = Z_;
}

// vi = iRi-1 vi-1 + iRi-1 (wi-1 x i-1Pi)
// wi = iRi-1 wi-1
// Vi = iXi-1 Vi-1
// iXi-1 = [iRi-1 -iRi-1(i-1Pi x); 0 iRi-1]
//       = [Rt -Rt (P x); 0 Rt] 
void PrLink::Xform6T(const PrVector6 &v6,PrVector6 &dest)
{
    PrVector3 &PxW = tmpV31;
    PrVector3 &V_minus_PxW = PxW;

    localT_.translation().cross(v6[1],PxW);
    v6[0].subtract(PxW,V_minus_PxW);
    Rt_.multiply(V_minus_PxW,dest[0]);
    Rt_.multiply(v6[1],dest[1]);
}

// fi-1 = i-1Ri fi
// ni-1 =  (i-1P x) i-1Ri fi + i-1Ri ni
// Fi-1 = i-1Xi Fi
// i-1Xi = [i-1Ri 0; (i-1Pi x)i-Ri i-1Ri]
//       = [R 0; (P x)R R] 
void PrLink::Xform6(const PrVector6 &f6,PrVector6 &dest)
{
    // f6 = [ F; N]
    PrVector3 &RN = tmpV31;

    R_.multiply(f6[0],dest[0]);
    localT_.translation().cross(dest[0],dest[1]);
    R_.multiply(f6[1],RN);
    dest[1] += RN;
}

// i-1Xi I (i-1Xi)^T = [R 0; (Px)R R][I0 I1; I2 I3][Rt -Rt(Px); 0 Rt]
// = [R I0  R I1; ((Px)R I0 + R I2) ((Px)R I1 + R I3)][Rt -Rt(Px); 0 Rt]
// = [X0 X1; X2 X3]
// X0 = R I0 Rt
// X1 = -R I0 Rt(Px) + R I1 Rt
// X2 = (Px)R I0 Rt + R I2 Rt
// X3 = -(Px)R I0 Rt(Px) - R I2 Rt(Px) + (Px)R I1 Rt + R I3 Rt
// if I is symmetric, (TRUE!)
// X0 = R I0 Rt
// X2 = (Px)X0 + R I2 Rt 
// X1 = X2^T
// X3 = (Px) X1 - R I2 Rt(Px) + R I3 Rt
void PrLink::similarityXform6(const PrMatrix6 &I,PrMatrix6 &XIXt)
{
    PrMatrix3 &RI = tmpM31;
    PrMatrix3 &RIRt = tmpM32;
    PrMatrix3 &RIRtPx = tmpM31;


    // X0 = R I0 Rt
    R_.multiply(I[0],RI);
    RI.multiply(Rt_,XIXt[0]);

    // X2 = (Px)X0 + R I2 Rt 
    Px_.multiply(XIXt[0],XIXt[2]);
    R_.multiply(I[2],RI);
    RI.multiply(Rt_,RIRt);
    XIXt[2] += RIRt;

    // X1 = X2^T
    XIXt[2].transpose(XIXt[1]);

    // X3 = (Px) X1 - R I2 Rt(Px) + R I3 Rt
    Px_.multiply(XIXt[1],XIXt[3]);
    RIRt.multiply(Px_,RIRtPx);
    XIXt[3] -= RIRtPx;
    R_.multiply(I[3],RI);
    RI.multiply(Rt_,RIRt);
    XIXt[3] += RIRt;
}

// iXh L (iXh)^T = [Rt 0; -Rt(Px) Rt][L0 L1; L2 L3][R (Px)R; 0 R]
// = [Rt L0  Rt L1; (-Rt(Px) L0 + Rt L2) (-Rt(Px) L1 + Rt L3)][R (Px)R; 0 R]
// = [X0 X1; X2 X3]
// X0 = Rt L0 R
// X1 = Rt L0 (Px)R + Rt L1 R
// X2 = -Rt(Px) L0 R + Rt L2 R
// X3 = -Rt(Px) L0 (Px)R + Rt L2 (Px)R - Rt(Px) L1 R + Rt L3 R
// if L is symmetric, (TRUE!)
// X0 = Rt L0 R
// X2 = Rt [L2 - (Px) L0] R
// X1 = X2^T
// X3 = -Rt(Px) L0 (Px)R + Rt L2 (Px)R - Rt(Px) L1 R + Rt L3 R
//    = Rt[-(Px)L0(Px) +  L2(Px) - (Px)L1 + L3]R
//    = Rt[-(Px)L0(Px) +  L2(Px) + (L2(Px))^T + L3]R
//    = -Rt(Px) L0 (Px)R + Rt L2 (Px)R + [Rt L2 (Px)R]^T + Rt L3 R
//    = Rt [L2 - (Px) L0] R Rt (Px) R + Rt [L3 - (Px) L1] R
//    = X[2] Rt (Px) R + Rt [L3 - (Px) L1] R
void PrLink::similarityXform6inv(const PrMatrix6 &L,PrMatrix6 &XinvLXinvt)
{

    PrMatrix3 &RtL = tmpM31;
    //    PrMatrix3 &RtLR = tmpM32;
    PrMatrix3 &PxL = tmpM31; 
    PrMatrix3 &LminusPxL = tmpM32;
    //    PrMatrix3 &RtPx = tmpM31;
    //    PrMatrix3 &RtPxR = tmpM32;
    PrMatrix3 &LPxT = tmpM31;
    PrMatrix3 &PxLPx = tmpM32;

    // X0 = Rt L0 R
    Rt_.multiply(L[0],RtL);
    RtL.multiply(R_,XinvLXinvt[0]);

    // X2 = Rt [L2 - (Px) L0] R
    LminusPxL = L[2];
    Px_.multiply(L[0],PxL);
    LminusPxL -= PxL;
    Rt_.multiply(LminusPxL,RtL);
    RtL.multiply(R_,XinvLXinvt[2]);

    // X1 = X2^T
    XinvLXinvt[2].transpose(XinvLXinvt[1]);

    // method1
    // most stable
    // X3 = Rt[-(Px)L0(Px) +  L2(Px) + (L2(Px))^T + L3]R
    L[2].multiply(Px_,XinvLXinvt[3]);
    XinvLXinvt[3].transpose(LPxT);
    XinvLXinvt[3]+=LPxT;
    //XinvLXinvt[3].display("L2(Px) + (L2(Px))^T");
    XinvLXinvt[3]+=L[3];
    //XinvLXinvt[3].display("L2(Px) + (L2(Px))^T + L3");
    Px_.multiply(L[0],PxL);
    PxL.multiply(Px_,PxLPx);
    XinvLXinvt[3]-=PxLPx;
    //XinvLXinvt[3].display("-(Px)L0(Px) + L2(Px) + (L2(Px))^T + L3");
    Rt_.multiply(XinvLXinvt[3],RtL);
    RtL.multiply(R_,XinvLXinvt[3]);
    //XinvLXinvt[3].display("RtLR");
    
    // method2
    // X3 = X[2] Rt (Px) R + Rt [L3 - (Px) L1] R
    /*
    Rt_.multiply(Px_,RtPx);
    RtPx.multiply(R_,RtPxR);
    XinvLXinvt[2].multiply(RtPxR,XinvLXinvt[3]);

    LminusPxL = L[3];
    Px_.multiply(L[1],PxL);
    LminusPxL -= PxL;
    Rt_.multiply(LminusPxL,RtL);
    RtL.multiply(R_,RtLR);
    XinvLXinvt[3] += RtLR;
    */
    // method3
    // X3 = [(Px)R]^T L0 (Px)R + Rt L2 (Px)R + [Rt L2 (Px)R]^T + Rt L3 R
    /*
    PrMatrix3 PxR;
    PrMatrix3 &PxRT = tmpM32;
    PrMatrix3 &PxRTL = tmpM31;
    PrMatrix3 &RtLPxR = tmpM32;
    PrMatrix3 &RtLPxRT = tmpM31;

    Px_.multiply(R_,PxR);
    PxR.transpose(PxRT);
    PxRT.multiply(L[0],PxRTL);
    PxRTL.multiply(PxR,XinvLXinvt[3]);

    Rt_.multiply(L[2],RtL);
    RtL.multiply(PxR,RtLPxR);
    XinvLXinvt[3]+=RtLPxR;

    RtLPxR.transpose(RtLPxRT);
    XinvLXinvt[3]+=RtLPxRT;

    Rt_.multiply(L[3],RtL);
    RtL.multiply(R_,RtLR);
    XinvLXinvt[3]+=RtLR;
    */
}




