bool GetPoseYaml( const YAML::Node& node, PoseSE3& pose )
{
if( !node["orientation"] || !node["position"] )
{
return false;
}
Eigen::Quaterniond quat;
if( !GetOrientationYaml( node["orientation"], quat ) )
{
EulerAngles eul;
if( !GetOrientationYaml( node["orientation"], eul ) ) { return false; }
quat = EulerToQuaternion( eul );
}
Eigen::Translation3d pos;
if( !GetPositionYaml( node["position"], pos ) ) { return false; }
pose = PoseSE3( pos, quat );
return true;
}
void Xform::updateSample(Time t_)
{
super::updateSample(t_);
if (m_update_flag.bits == 0) {
m_sample.flags = (m_sample.flags & ~(int)XformData::Flags::UpdatedMask);
return;
}
if (m_update_flag.variant_set_changed) { m_summary_needs_update = true; }
auto t = UsdTimeCode(t_);
const auto& conf = getImportSettings();
auto& sample = m_sample;
auto prev = sample;
if (m_summary.type == XformSummary::Type::TRS) {
auto translate = float3::zero();
auto scale = float3::one();
auto rotation = quatf::identity();
for (auto& op : m_read_ops) {
switch (op.GetOpType()) {
case UsdGeomXformOp::TypeTranslate:
{
float3 tmp;
op.GetAs((GfVec3f*)&tmp, t);
translate += tmp;
break;
}
case UsdGeomXformOp::TypeScale:
{
float3 tmp;
op.GetAs((GfVec3f*)&tmp, t);
scale *= tmp;
break;
}
case UsdGeomXformOp::TypeOrient:
{
quatf tmp;
op.GetAs((GfQuatf*)&tmp, t);
rotation *= tmp;
break;
}
case UsdGeomXformOp::TypeRotateX:
{
float angle;
op.GetAs(&angle, t);
rotation *= rotateX(angle * Deg2Rad);
break;
}
case UsdGeomXformOp::TypeRotateY:
{
float angle;
op.GetAs(&angle, t);
rotation *= rotateY(angle * Deg2Rad);
break;
}
case UsdGeomXformOp::TypeRotateZ:
{
float angle;
op.GetAs(&angle, t);
rotation *= rotateZ(angle * Deg2Rad);
break;
}
case UsdGeomXformOp::TypeRotateXYZ: //
case UsdGeomXformOp::TypeRotateXZY: //
case UsdGeomXformOp::TypeRotateYXZ: //
case UsdGeomXformOp::TypeRotateYZX: //
case UsdGeomXformOp::TypeRotateZXY: //
case UsdGeomXformOp::TypeRotateZYX: // fall through
{
float3 euler;
op.GetAs((GfVec3f*)&euler, t);
rotation *= EulerToQuaternion(euler * Deg2Rad, op.GetOpType());
break;
}
default:
break;
}
}
if (conf.swap_handedness) {
translate.x *= -1.0f;
rotation = swap_handedness(sample.rotation);
}
sample.position = translate;
sample.rotation = rotation;
sample.scale = scale;
}
else {
GfMatrix4d result;
result.SetIdentity();
for (auto& op : m_read_ops) {
auto m = op.GetOpTransform(t);
result = m * result;
}
GfTransform gft;
gft.SetMatrix(result);
(GfMatrix4f&)sample.transform = GfMatrix4f(result);
(GfVec3f&)sample.position = GfVec3f(gft.GetTranslation());
(GfQuatf&)sample.rotation = GfQuatf(gft.GetRotation().GetQuat());
(GfVec3f&)sample.scale = GfVec3f(gft.GetScale());
}
int update_flags = 0;
if (!near_equal(prev.position, sample.position)) {
update_flags |= (int)XformData::Flags::UpdatedPosition;
}
if (!near_equal(prev.rotation, sample.rotation)) {
update_flags |= (int)XformData::Flags::UpdatedRotation;
}
if (!near_equal(prev.scale, sample.scale)) {
update_flags |= (int)XformData::Flags::UpdatedScale;
}
sample.flags = (sample.flags & ~(int)XformData::Flags::UpdatedMask) | update_flags;
}
