public emVector3 quatRotate(emQuaternion rotation, emVector3 v)
{
emQuaternion q = rotation * v;
q = q * rotation.inverse();
return(new emVector3(q.x, q.y, q.z));
}
public override void finalize(WalkEnding end, ulong _time)
{
switch (end)
{
case WalkEnding.Identity:
break;
case WalkEnding.TargetParentOfSource:
result_vec = source_to_top_vec;
result_quat = source_to_top_quat;
break;
case WalkEnding.SourceParentOfTarget:
{
emQuaternion inv_target_quat = target_to_top_quat.inverse();
emVector3 inv_target_vec = quatRotate(inv_target_quat, -1 * target_to_top_vec);
result_quat = inv_target_quat;
result_vec = inv_target_vec;
}
break;
case WalkEnding.FullPath:
{
emQuaternion inv_target_quat = target_to_top_quat.inverse();
emVector3 inv_target_vec = quatRotate(inv_target_quat, new emVector3(-target_to_top_vec.x, -target_to_top_vec.y, -target_to_top_vec.z));
result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
result_quat = inv_target_quat * source_to_top_quat;
}
break;
}
time = _time;
}
public TransformStorage(emTransform data, uint frame_id, uint child_frame_id)
{
rotation = data.rotation;
translation = data.translation;
stamp = TimeCache.toLong(data.stamp.data);
this.frame_id = frame_id;
this.child_frame_id = child_frame_id;
}
public emTransform(emQuaternion q, emVector3 v, Time t, string fid, string cfi)
{
rotation = q;
translation = v;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public double angleShortestPath(emQuaternion q)
{
double s = Math.Sqrt(length2() * q.length2());
if (dot(q) < 0)
{
return(Math.Acos(dot(-1 * q) / s) * 2.0);
}
return(Math.Acos(dot(q) / s) * 2.0);
}
public void setRotation(emQuaternion q)
{
double d = q.length2();
double s = 2.0 / d;
double xs = q.x * s, ys = q.y * s, zs = q.z * s;
double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
setValue(1.0 - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0 - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0 - (xx + yy));
}
public override void accum(bool source)
{
if (source)
{
source_to_top_vec = quatRotate(st.rotation, source_to_top_vec) + st.translation;
source_to_top_quat = st.rotation * source_to_top_quat;
}
else
{
target_to_top_vec = quatRotate(st.rotation, target_to_top_vec) + st.translation;
target_to_top_quat = st.rotation * target_to_top_quat;
}
}
public emQuaternion slerp(emQuaternion q, double t)
{
double theta = angleShortestPath(q);
if (theta != 0)
{
double d = 1.0 / Math.Sin(theta);
double s0 = Math.Sin(1.0 - t) * theta;
double s1 = Math.Sin(t * theta);
if (dot(q) < 0)
{
return(new emQuaternion((x * s0 + -1 * q.x * s1) * d,
(y * s0 + -1 * q.y * s1) * d,
(z * s0 + -1 * q.z * s1) * d,
(w * s0 + -1 * q.w * s1) * d));
}
return(new emQuaternion((x * s0 + q.x * s1) * d,
(y * s0 + q.y * s1) * d,
(z * s0 + q.z * s1) * d,
(w * s0 + q.w * s1) * d));
}
return(new emQuaternion(this));
}
public emMatrix3x3(emQuaternion q)
{
setRotation(q);
}
public TransformStorage()
{
rotation = new emQuaternion();
translation = new emVector3();
}
private emQuaternion slerp(emQuaternion q1, emQuaternion q2, double rt)
{
return(q1.slerp(q2, rt));
}
public double dot(emQuaternion q)
{
return(x * q.x + y * q.y + z * q.z + w * q.w);
}
public emQuaternion(emQuaternion shallow) : this(shallow.x, shallow.y, shallow.z, shallow.w)
{
}
