public emVector3 quatRotate(emQuaternion rotation, emVector3 v)
{
emQuaternion q = rotation * v;
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, -1 * target_to_top_vec);
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 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, -1 * target_to_top_vec);
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;
}
// Update is called once per frame
protected override void Update()
{
lock (currentMsg)
{
lock (path)
{
while (path.Count > pointCount)
{
Destroy(path[0]);
path.RemoveAt(0);
}
while (path.Count < pointCount)
{
GameObject go = GameObject.CreatePrimitive(PrimitiveType.Sphere);
go.hideFlags |= HideFlags.HideInHierarchy;
path.Add(go);
}
for (int index = 0; index < pointCount; ++index)
{
path[index].transform.localScale = new Vector3(pointSize, pointSize, pointSize);
tf.net.emVector3 pos = new tf.net.emVector3((float)currentMsg.poses[index].pose.position.x, currentMsg.poses[index].pose.position.y, currentMsg.poses[index].pose.position.z);
path[index].transform.position = pos.UnityPosition + Offset;
path[index].GetComponent <MeshRenderer>().material.color = Color;
}
}
}
}
public emTransform(emQuaternion q, emVector3 v, Time t = null, string fid = null, string cfi = null)
{
basis = q;
origin = v;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public emMatrix3x3(double xx, double xy, double xz,
double yx, double yy, double yz,
double zx, double zy, double zz)
{
m_el[0] = new emVector3(xx, xy, xz);
m_el[1] = new emVector3(yx, yy, yz);
m_el[2] = new emVector3(zx, zy, zz);
}
public void setValue(double xx, double xy, double xz,
double yx, double yy, double yz,
double zx, double zy, double zz)
{
m_el[0] = new emVector3(xx, xy, xz);
m_el[1] = new emVector3(yx, yy, yz);
m_el[2] = new emVector3(zx, zy, zz);
}
public void setValue(double xx, double xy, double xz,
double yx, double yy, double yz,
double zx, double zy, double zz)
{
m_el[0] = new emVector3(xx, xy, xz);
m_el[1] = new emVector3(yx, yy, yz);
m_el[2] = new emVector3(zx, zy, zz);
}
public void setInterpolate3(emVector3 v0, emVector3 v1, double rt)
{
double s = 1.0 - rt;
x = s * v0.x + rt * v1.x;
y = s * v0.y + rt * v1.y;
z = s * v0.z + rt * v1.z;
}
public emMatrix3x3(double xx, double xy, double xz,
double yx, double yy, double yz,
double zx, double zy, double zz)
{
m_el[0] = new emVector3(xx, xy, xz);
m_el[1] = new emVector3(yx, yy, yz);
m_el[2] = new emVector3(zx, zy, zz);
}
public TransformStorage(emTransform data, uint frame_id, uint child_frame_id)
{
rotation = data.basis;
translation = data.origin;
stamp = TimeCache.toLong(data.stamp.data);
this.frame_id = frame_id;
this.child_frame_id = child_frame_id;
}
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 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 void transformVector(string target_frame, Stamped <emVector3> stamped_in, ref Stamped <emVector3> stamped_out)
{
emTransform trans = new emTransform();
lookupTransform(target_frame, stamped_in.frame_id, stamped_in.stamp, out trans);
emVector3 end = stamped_in.data;
emVector3 origin = new emVector3(0, 0, 0);
emVector3 output = (trans * end) - (trans * origin);
stamped_out.data = output;
stamped_out.stamp = trans.stamp;
stamped_out.frame_id = target_frame;
}
public static emQuaternion FromRPY(emVector3 rpy)
{
double halfroll = rpy.x / 2;
double halfpitch = rpy.y / 2;
double halfyaw = rpy.z / 2;
double sin_r2 = Math.Sin(halfroll);
double sin_p2 = Math.Sin(halfpitch);
double sin_y2 = Math.Sin(halfyaw);
double cos_r2 = Math.Cos(halfroll);
double cos_p2 = Math.Cos(halfpitch);
double cos_y2 = Math.Cos(halfyaw);
return(new emQuaternion(
cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2,
sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2,
cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2,
cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2
));
}
public emTransform(emQuaternion q, emVector3 v, Time t = null, string fid = null, string cfi = null)
{
basis = q;
origin = v;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public emVector3(emVector3 shallow) : this(shallow.x, shallow.y, shallow.z)
{
}
public double dot(emVector3 v2)
{
return x*v2.x + y*v2.y + z*v2.z;
}
public double dot(emVector3 v2)
{
return(x * v2.x + y * v2.y + z * v2.z);
}
public emVector3(emVector3 shallow) : this(shallow.x, shallow.y, shallow.z)
{
}
public void setInterpolate3(emVector3 v0, emVector3 v1, double rt)
{
double s = 1.0 - rt;
x = s*v0.x + rt*v1.x;
y = s*v0.y + rt*v1.y;
z = s*v0.z + rt*v1.z;
}
public static emQuaternion FromRPY(emVector3 rpy)
{
double halfroll = rpy.x/2;
double halfpitch = rpy.y/2;
double halfyaw = rpy.z/2;
double sin_r2 = Math.Sin(halfroll);
double sin_p2 = Math.Sin(halfpitch);
double sin_y2 = Math.Sin(halfyaw);
double cos_r2 = Math.Cos(halfroll);
double cos_p2 = Math.Cos(halfpitch);
double cos_y2 = Math.Cos(halfyaw);
return new emQuaternion(
cos_r2*cos_p2*cos_y2 + sin_r2*sin_p2*sin_y2,
sin_r2*cos_p2*cos_y2 - cos_r2*sin_p2*sin_y2,
cos_r2*sin_p2*cos_y2 + sin_r2*cos_p2*sin_y2,
cos_r2*cos_p2*sin_y2 - sin_r2*sin_p2*cos_y2
);
}
public emVector3 quatRotate(emQuaternion rotation, emVector3 v)
{
emQuaternion q = rotation * v;
q *= rotation.inverse();
return new emVector3(q.x, q.y, q.z);
}
public TransformStorage()
{
rotation = new emQuaternion();
translation = new emVector3();
}
