Hi, I was trying to get some rotations to work and couldn’t figure out how to use the skeleton orientation right. I’ve tried to do a little code which converts the 3 orientations to a quaternion, which then can be used with the Quaternion (Rotate) node. It seems to work somehow, sometimes though the rotations might be inverted etc. Maybe someone with some better than my poor math skills want to check it. Could also be nice to have something like this as a single output on the Skeleton node. If Iam totally wrong and someone wants to explain how to use the orientations right, I would also be very glad. Thanks
- region usings
using System;
using System.ComponentModel.Composition;
using VVVV.PluginInterfaces.V1;
using VVVV.PluginInterfaces.V2;
using VVVV.Utils.VColor;
using VVVV.Utils.VMath;
using VVVV.Core.Logging;
- endregion usings
namespace VVVV.Nodes
{
#region PluginInfo
[PluginInfo(Name = "OrientationToQuaternion", Category = "Kinect", Help = "transforms Kinect orientation to Quaternion", Tags = "quaternion")](PluginInfo(Name = "OrientationToQuaternion", Category = "Kinect", Help = "transforms Kinect orientation to Quaternion", Tags = "quaternion"))
#endregion PluginInfo
public class KinectOrientationToQuaternionNode : IPluginEvaluate
{
#region fields & pins
[Input("OrientationX")](Input("OrientationX"))
ISpread<Vector3D> FJointOrientationXIn;
[Input("OrientationY")](Input("OrientationY"))
ISpread<Vector3D> FJointOrientationYIn;
[Input("OrientationZ")](Input("OrientationZ"))
ISpread<Vector3D> FJointOrientationZIn;
[Output("QuaternionOUT")](Output("QuaternionOUT"))
ISpread<Vector4D> FQuaternionOut;
[Import()](Import())
ILogger FLogger;
#endregion fields & pins
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
int inputCount = FJointOrientationXIn.SliceCount;
FQuaternionOut.SliceCount = inputCount;
for(int x=0; x<inputCount; x++)
{
Matrix4x4 o = new Matrix4x4();
o.row1 = new Vector4D(FJointOrientationXIn[x](x).x, FJointOrientationXIn[x](x).y, FJointOrientationXIn[x](x).z, 0);
o.row2 = new Vector4D(FJointOrientationYIn[x](x).x, FJointOrientationYIn[x](x).y, FJointOrientationYIn[x](x).z, 0);
o.row3 = new Vector4D(FJointOrientationZIn[x](x).x, FJointOrientationZIn[x](x).y, FJointOrientationZIn[x](x).z, 0);
double tr = FJointOrientationXIn[x](x).x + FJointOrientationYIn[x](x).y + FJointOrientationZIn[x](x).z;
double qw = 0.0d;
double qx = 0.0d;
double qy = 0.0d;
double qz = 0.0d;
if(tr > 0)
{
double S = Math.Sqrt(tr+1.0d) * 2.0d;
qw = 0.25d * S;
qx = (o.m23 - o.m32) / S;
qy = (o.m31 - o.m13) / S;
qz = (o.m12 - o.m21) / S;
}else if[o.m11 > o.m22) && (o.m11 > o.m33](https://vvvv.org/documentation/o.m11->-o.m22)-&&-(o.m11->-o.m33)
{
double S = Math.Sqrt(1.0d + o.m11 - o.m22 - o.m33) * 2.0d;
qw = (o.m33 - o.m32) / S;
qx = 0.25d * S;
qy = (o.m21 + o.m12) / S;
qz = (o.m31 + o.m13) / S;
}else if(o.m22 > o.m33)
{
double S = Math.Sqrt(1.0d + o.m22 - o.m11 - o.m33) * 2.0d;
qw = (o.m31 - o.m13) / S;
qx = (o.m21 + o.m22) / S;
qy = 0.25d * S;
qz = (o.m32 + o.m23) / S;
}else{
double S = Math.Sqrt(1.0d + o.m33 - o.m11 - o.m22) * 2.0d;
qw = (o.m12 - o.m21) / S;
qx = (o.m31 + o.m13) / S;
qy = (o.m32 + o.m23) / S;
qz = 0.25d * S;
}
FQuaternionOut[x](x) = new Vector4D(qx, qy, qz, qw);
}
}
}
}