public void LDiv(ref QPoint q)
{
QPoint qc = q;
qc.Conj();
this.LMul(ref qc);
}
//
// res(L) = CTrans(tr,rt)(this(L))
public CTrans MakeApplyRt(ref RPoint tr, ref QPoint rt)
{
CTrans res = new CTrans(this);
res.ApplyRt(ref tr, ref rt);
return(res);
}
public void RMul(ref QPoint q)
{
QPoint qc = q;
qc.LMul(ref this);
this = qc;
}
// l=r*(w)*r^(-1)
public void LtoW_V(ref RPoint LP, out RPoint WP)
{
QPoint Q = new QPoint(LP, 0.0);
Q.LMul(ref m_R);
Q.RDiv(ref m_R);
WP = Q.PT;
}
// l=r^(-1)*(w)*r
public void WtoL_V(ref RPoint WP, out RPoint LP)
{
QPoint Q = new QPoint(WP, 0.0);
Q.RMul(ref m_R);
Q.LDiv(ref m_R);
LP = Q.PT;
}
// |b|=1
public void RDiv(ref QPoint q)
{
QPoint qc = q;
qc.Conj();
qc.LMul(ref this);
this = qc;
}
// a=b*a
public void LMul(ref QPoint q)
{
double r = R * q.R - X * q.X - Y * q.Y - Z * q.Z;
double x = X * q.R + R * q.X + Z * q.Y - Y * q.Z;
double y = Y * q.R + R * q.Y + X * q.Z - Z * q.X;
double z = Z * q.R + R * q.Z + Y * q.X - X * q.Y;
R = r; PT.X = x; PT.Y = y; PT.Z = z;
}
//
// new(L) = par(this(L))
protected void ApplyRt(ref RPoint tr, ref QPoint rt)
{
QPoint QT = new QPoint(m_T, 0);
QT.LMul(ref rt);
QT.RDiv(ref rt);
m_T = QT.PT;
m_T.Add(ref tr);
m_R.LMul(ref rt);
}
// res=this'; L=this(res(L))=res(this(L))
public CTrans MakeInverse()
{
QPoint QT = new QPoint(RPoint.Zero - this.T, 0);
QT.RMul(ref this.m_R);
QT.LDiv(ref this.m_R);
CTrans res = new CTrans(QT.PT, this.m_R);
res.m_R.Conj();
return(res);
}
static public CTrans Matrix2CTrans(SharpDX.Matrix M)
{
RPoint T = new RPoint(
-M.M41,
M.M42,
M.M43);
M.M41 = M.M42 = M.M43 = 0;
Quaternion qRotation = Quaternion.RotationMatrix(M);
QPoint R = new QPoint(
-qRotation.X,
qRotation.Y,
qRotation.Z,
-qRotation.W);
return(new CTrans(T, R));
}
//
public static CTrans FromAxis(RPoint pt, RPoint vec, double an)
{
an *= Math.PI / 360.0; // half in radians
RPoint T = RPoint.Zero;
RPoint C = pt;
double lR = vec.Dist();
double xc = Math.Cos(an), xs = Math.Sin(an);
vec.Mul(xs / lR);
QPoint R = new QPoint(vec, xc);
R.Norm();
// w=R^(-1)*(l-C)*R+C
T.Add(ref C);
QPoint QC = new QPoint(C, 0);
QC.LMul(ref R);
QC.RDiv(ref R);
T.Sub(ref QC.PT);
return(new CTrans(T, R));
}
public CTrans(CTrans ct)
{
this.m_T = ct.T;
this.m_R = ct.R; // no Norm needed here
}
//
public CTrans(RPoint shiftV, QPoint rotQ)
{
this.T = shiftV;
this.R = rotQ; //! Norm
}
public void Sub(ref QPoint q)
{
PT.Sub(ref q.PT);
R -= q.R;
}
// destructive ops
public void Add(ref QPoint q)
{
PT.Add(ref q.PT);
R += q.R;
}
//
public bool Equals(QPoint qpt)
{
return(this == qpt);
}