// Qt Streams
QDebug operator<<(QDebug dbg, const Transform3D &transform)
{
dbg << "Transform3D\n{\n";
dbg << "Position: <" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << ">\n";
dbg << "Scale: <" << transform.scale().x() << ", " << transform.scale().y() << ", " << transform.scale().z() << ">\n";
dbg << "Rotation: <" << transform.rotation().x() << ", " << transform.rotation().y() << ", " << transform.rotation().z() << " | " << transform.rotation().scalar() << ">\n}";
return dbg;
}
float Transform3D::norm(Transform3D T){
float pos_norm = arma::norm(T.translation());
UnitQuaternion q = UnitQuaternion(Rotation3D(T.submat(0,0,2,2)));
float angle = q.getAngle();
//TODO: how to weight these two?
return pos_norm + Rotation3D::norm(T.rotation());
}
Transform3D Transform3D::interpolate(Transform3D T1, Transform3D T2, float alpha){
Rotation3D r1 = T1.rotation();
UnitQuaternion q1 = UnitQuaternion(r1);
Rotation3D r2 = T2.rotation();
UnitQuaternion q2 = UnitQuaternion(r2);
arma::vec3 t1 = T1.translation();
arma::vec3 t2 = T2.translation();
UnitQuaternion qResult = q1.slerp(q2,alpha);
arma::vec3 tResult = alpha * (t2 - t1) + t1;
Transform3D TResult = Transform3D(Rotation3D(qResult));
TResult.translation() = tResult;
return TResult;
}
float Transform3D::norm(Transform3D T) {
float pos_norm = arma::norm(T.translation());
// return Rotation3D::norm(T.rotation());
// TODO: how to weight these two?
return pos_norm + Rotation3D::norm(T.rotation());
}