public DualQuaternion(Vector3 axis, Vector3 Vector3, float angle, Vector3 offset)
{
axis = axis.Normalized;
var qR = new DualQuaternion(axis, angle, offset);
var qT = new DualQuaternion(Vector3);
var res = qT * qR * qT.Conjugate;
Real = res.Real;
Dual = res.Dual;
}
public void Draw(Shader shader)
{
Dispatcher.UpdateConfig.Reset();
Vector4[] axes = new Vector4[Links.Length];
Vector4[] pos = new Vector4[Links.Length];
axes[0] = Vector4.UnitY;
pos[0] = new Vector4(Vector3.Zero, 1);
shader.Use();
//joints[0].q = time;
//Joints[1].q += 0.016;
//joints[2].q = time;
Matrix4 model;
var quat = DualQuaternion.Default;
// joints
for (int i = 0; i < Links.Length; i++)
{
quat *= new DualQuaternion(Vector3.UnitY, -(float)DH[i].theta);
model = quat.Matrix;
shader.SetMatrix4("model", model, true);
Joints[i].Model.Position = (Vector3)(model * new Vector4(Joints[0].Model.Position, 1.0f));
Joints[i].Model.Draw(shader, MeshMode.Solid | MeshMode.Wireframe);
quat *= new DualQuaternion((float)DH[i].d * Vector3.UnitY);
quat *= new DualQuaternion(Vector3.UnitX, (float)DH[i].alpha, (float)DH[i].r * Vector3.UnitX);
model = quat.Matrix;
if (i < Links.Length - 1)
{
axes[i + 1] = model * axes[0];
pos[i + 1] = new Vector4(model.M14, model.M24, model.M34, 1);
}
}
quat = DualQuaternion.Default;
// links
quat *= new DualQuaternion(Joints[0].Length / 2 * Vector3.UnitY);
// the order of multiplication is reversed, because the trans quat transforms the operand (quat) itself; it does not contribute in total transformation like other quats do
var trans_quat = new DualQuaternion(axes[0].Xyz, pos[0].Xyz, -(float)DH[0].theta);
quat = trans_quat * quat;
model = quat.Matrix;
shader.SetMatrix4("model", model, true);
Links[0].Model.Draw(shader, MeshMode.Solid | MeshMode.Wireframe);
quat *= new DualQuaternion((float)DH[0].d * Vector3.UnitY);
trans_quat = new DualQuaternion(axes[1].Xyz, pos[1].Xyz, -(float)(DH[1].theta + Math.PI / 2));
quat = trans_quat * quat;
model = quat.Matrix;
shader.SetMatrix4("model", model, true);
Links[1].Model.Draw(shader, MeshMode.Solid | MeshMode.Wireframe);
quat *= new DualQuaternion((float)DH[1].r * Vector3.UnitY);
trans_quat = new DualQuaternion(axes[2].Xyz, pos[2].Xyz, -(float)DH[2].theta);
quat = trans_quat * quat;
model = quat.Matrix;
shader.SetMatrix4("model", model, true);
Links[2].Model.Draw(shader, MeshMode.Solid | MeshMode.Wireframe);
Dispatcher.UpdateConfig.Set();
}