/***************************************************************
 * PrTransform.cpp
 *
 * This implements a homogeneous transformation=[R,P] class.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/20/97: K.C. Chang: added inline methods.
 * 11/05/97: K.C. Chang: created.
 */

#include "PrTransform.h"
#include <stdio.h>

// ~[r,p] = [~r, -(~r*p)]
PrTransform PrTransform::operator~() const
{
    PrTransform tmpT;
    inverse(tmpT);
    return tmpT;
}

void PrTransform::inverse(PrTransform &dest) const
{
    rot_.inverse(dest.rot_);
    dest.rot_.multiply(trans_,dest.trans_);
    dest.trans_.negate(dest.trans_);
}

void PrTransform::error(const PrTransform &Td,PrVector6 &e)
{
  trans_.subtract(Td.trans_,e[0]);
  rot_.angularError(Td.rot_,e[1]);
}

// [r1,p1][r2,p2] = [r1*r2, r1*p2 + p1]
PrTransform PrTransform::operator*(const PrTransform &t) const
{
    PrTransform tmpT;
    multiply(t,tmpT);
    return tmpT;
}

void PrTransform::multiply(const PrTransform &t,PrTransform &dest) const
{
    rot_.multiply(t.rot_,dest.rot_);
    rot_.multiply(t.trans_,dest.trans_);
    dest.trans_ += trans_;
}

// [r,p]v = r*v + p
PrVector3 PrTransform::operator*(const PrVector3 &v) const
{
    PrVector3 tmpV;
    multiply(v,tmpV);
    return tmpV;
}

void PrTransform::multiply(const PrVector3 &v,PrVector3 &dest) const
{
    rot_.multiply(v,dest);
    dest += trans_;
}

PrTransform &PrTransform::operator*=(const PrTransform &t)
{
    PrTransform tmpT;
    multiply(t,tmpT);
    (*this) = tmpT;
    return (*this);
}

// 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] 
PrVector6 PrTransform::XformT(const PrVector6 &v6) const
{
    PrVector6 tmpV;
    XformT(v6,tmpV);
    return tmpV;
}

void PrTransform::XformT(const PrVector6 &v6,PrVector6 &dest) const
{
    PrVector3 tmpV;

    trans_.cross(v6[1],tmpV);
    v6[0].subtract(tmpV,tmpV);
    (~rot_).multiply(tmpV,dest[0]);
    (~rot_).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] 
PrVector6 PrTransform::Xform(const PrVector6 &f6) const
{
    PrVector6 tmpV;
    Xform(f6,tmpV);
    return tmpV;
}

void PrTransform::Xform(const PrVector6 &f6,PrVector6 &dest) const
{
    // f6 = [ F; N]
    PrVector3 tmpV;

    rot_.multiply(f6[0],dest[0]);
    trans_.cross(dest[0],dest[1]);
    rot_.multiply(f6[1],tmpV);
    dest[1] += tmpV;
}

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

PrMatrix6 PrTransform::similarityXform(const PrMatrix6 &I,
				       int symmetric) const
{
    PrMatrix6 tmpM;
    similarityXform(I,tmpM,symmetric);
    return tmpM;
}

void PrTransform::similarityXform(const PrMatrix6 &I,PrMatrix6 &XIXt,
				  int symmetric) const
{
    PrMatrix3 R;
    PrMatrix3 Rt;
    PrMatrix3 Px;
    PrMatrix3 RIRt;
    PrMatrix3 tmpM;

    rot_.matrix(R);
    R.transpose(Rt);
    trans_.cross(Px);

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

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

    if (symmetric == PR_MATRIX_SYMMETRIC)
      {
	  // X1 = X2^T
	  XIXt[2].transpose(XIXt[1]);
      }
    else
      {
	  // X1 = -R I0 Rt(Px) + R I1 Rt
	  R.multiply(I[1],tmpM);
	  tmpM.multiply(Rt,XIXt[1]);
	  XIXt[0].multiply(Px,tmpM);
	  XIXt[1] -= tmpM;
      }

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

void PrTransform::display(char *name)
{
    if (name != NULL)
      printf("%s =\n",name);
 
    printf("[rot=");
    rot_.display();
    printf(",\n trans=");
    trans_.display();
    printf("]\n");
}
