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(); }