public void Merge(ActorBehavior behaviour)
{
Vector3 rootRotation = new Vector3(rotation);
Vector3 rootTranslation = new Vector3(translation);
DualQuaternion rootTransform = DualQuaternion.FromRotationTranslation(
behaviour.model.MainDefinition.BoneSystem.RootBone.RotationOrder.FromEulerAngles(MathExtensions.DegreesToRadians(rootRotation)),
rootTranslation);
behaviour.dragHandle.Transform = rootTransform.ToMatrix();
var poseDeltas = behaviour.ikAnimator.PoseDeltas;
poseDeltas.ClearToZero();
foreach (var bone in behaviour.model.MainDefinition.BoneSystem.Bones)
{
Vector3 angles;
if (boneRotations.TryGetValue(bone.Name, out var values))
{
angles = new Vector3(values);
}
else
{
angles = Vector3.Zero;
}
var twistSwing = bone.RotationOrder.FromTwistSwingAngles(MathExtensions.DegreesToRadians(angles));
poseDeltas.Rotations[bone.Index] = twistSwing;
}
}
void drawGizmos(Color color)
{
var oldColor = Gizmos.color;
Gizmos.color = color;
var pos = transform.position;
var rot = transform.rotation;
var q = DualQuaternion.fromUnityTransform(rot, pos); //new DualQuaternion(rot.conjugate(), pos);//o_O Well, whatever.
drawDualQuaternion(q);
foreach (Transform cur in transform)
{
var childQ = DualQuaternion.fromUnityTransform(cur.transform.localRotation, cur.transform.localPosition);
//new DualQuaternion(cur.transform.localRotation.conjugate(), cur.transform.localPosition);
var childQ2 = q * childQ;
drawDualQuaternion(childQ2);
const int numSteps = 10;
for (int i = 1; i < numSteps; i++)
{
float t = (float)i / (float)numSteps;
var tmpQ = q.sclerp(childQ2, t);
drawDualQuaternion(tmpQ);
}
}
Gizmos.color = oldColor;
}
public PoseRecipe RecipizePose()
{
var rootTransform = DualQuaternion.FromMatrix(dragHandle.Transform);
Vector3 rootRotation = MathExtensions.RadiansToDegrees(model.MainDefinition.BoneSystem.RootBone.RotationOrder.ToEulerAngles(rootTransform.Rotation));
Vector3 rootTranslation = rootTransform.Translation;
Dictionary <string, float[]> boneRotations = new Dictionary <string, float[]>();
var poseDeltas = ikAnimator.PoseDeltas;
rootTranslation += Vector3.Transform(poseDeltas.RootTranslation / 100, rootTransform.Rotation);
foreach (var bone in model.MainDefinition.BoneSystem.Bones)
{
var twistSwing = poseDeltas.Rotations[bone.Index];
var angles = MathExtensions.RadiansToDegrees(bone.RotationOrder.ToTwistSwingAngles(twistSwing));
if (!angles.IsZero)
{
boneRotations.Add(bone.Name, angles.ToArray());
}
}
return(new PoseRecipe {
rotation = rootRotation.ToArray(),
translation = rootTranslation.ToArray(),
boneRotations = boneRotations
});
}
private void MaybeStartTracking(FrameUpdateParameters updateParameters, RigidBoneSystemInputs inputs, ControlVertexInfo[] previousFrameControlVertexInfos)
{
if (tracking == true)
{
//already tracking
return;
}
if (!stateTracker.NonMenuActive)
{
return;
}
bool triggerPressed = stateTracker.IsPressed(EVRButtonId.k_EButton_SteamVR_Trigger);
if (!triggerPressed)
{
return;
}
tracking = true;
TrackedDevicePose_t gamePose = updateParameters.GamePoses[stateTracker.DeviceIdx];
Matrix controllerTransform = gamePose.mDeviceToAbsoluteTracking.Convert();
DualQuaternion controllerTransformDq = DualQuaternion.FromMatrix(controllerTransform);
var worldSourcePosition = controllerTransformDq.Translation * 100;
var worldSourceOrientation = controllerTransformDq.Rotation;
sourceBone = parentInstance.MapPositionToBone(worldSourcePosition, previousFrameControlVertexInfos);
var inverseSourceBoneTotalTransform = sourceBone.GetChainedTransform(inputs).Invert();
boneRelativeSourcePosition = inverseSourceBoneTotalTransform.Transform(worldSourcePosition) - sourceBone.CenterPoint;
boneRelativeSourceOrientation = worldSourceOrientation.Chain(inverseSourceBoneTotalTransform.Rotation);
}
private InverseKinematicsGoal MaybeContinueTracking(FrameUpdateParameters updateParameters)
{
if (!tracking)
{
return(null);
}
if (!stateTracker.NonMenuActive)
{
tracking = false;
return(null);
}
bool triggerPressed = stateTracker.IsPressed(EVRButtonId.k_EButton_SteamVR_Trigger);
if (!triggerPressed)
{
tracking = false;
return(null);
}
TrackedDevicePose_t gamePose = updateParameters.GamePoses[stateTracker.DeviceIdx];
Matrix controllerTransform = gamePose.mDeviceToAbsoluteTracking.Convert();
var controllerTransformDq = DualQuaternion.FromMatrix(controllerTransform);
var targetPosition = controllerTransformDq.Translation * 100;
var targetOrientation = controllerTransformDq.Rotation;
return(new InverseKinematicsGoal(sourceBone,
boneRelativeSourcePosition, boneRelativeSourceOrientation,
targetPosition, targetOrientation));
}
public Vector3 transformPoint(Vector3 arg)
{
var val = new DualQuaternion(new Quaternion(0.0f, 0.0f, 0.0f, 1.0f), new Quaternion(arg.x, arg.y, arg.z, 0.0f));
var result = this * val *this.translationConjugate();
return(new Vector3(result.dual.x, result.dual.y, result.dual.z));
}
/// <summary>
/// Compute the coordinates of the end activator for a EPSON SCARA robot.
/// </summary>
/// <param name="pose">
/// The robot pose.
/// </param>
/// <returns>
/// The coordinates of the end effector.
/// </returns>
private static Vector3 ScaraForwardKinematics(ScaraRobotPose pose)
{
// Here we create the three rotation quaternions
// Note that we are directly using the Plücker coordinates of the joint axes.
// However, rotation would work for axes defined by an orientation and a point belonging to the line.
var r1 = DualQuaternion.CreateRotation(pose.Theta1, Vector3.UnitZ, new Vector3(0, 0, 0));
var r2 = DualQuaternion.CreateRotation(pose.Theta2, Vector3.UnitZ, new Vector3(300, 0, 0));
var r3 = DualQuaternion.CreateRotation(pose.Theta3, Vector3.UnitZ, new Vector3(650, 0, 0));
// Fourth joint is translation
var t = DualQuaternion.CreateTranslation(pose.Z, -Vector3.UnitZ);
// The end effector position for the reference configuration
var endEffector = DualQuaternion.CreateTranslation(new Vector3(650, 0, 318));
// Here we combine the four displacements into a single one and return it.
// Notice we also append the end-effector at the reference configuration.
// The order of application of the transformations is right-most is applied first.
var displacements = DualQuaternion.Multiply(r1, r2, r3, t, endEffector);
// Here we basically use the action f4g (specified in libdq’s manual) to transform the origin point.
// This will give us the forward kinematics of the robot.
var result = displacements.TransformPoint();
return(result);
}
public DualQuaternion Invert()
{
var inverseRotation = Quaternion.Invert(Rotation);
var inverseTranslation = Vector3.Transform(-Translation, inverseRotation);
return(DualQuaternion.FromRotationTranslation(inverseRotation, inverseTranslation));
}
public Matrix4x4 ToMatrix()
{
DualQuaternion q = Normalized();
Matrix4x4 M = Matrix4x4.identity;
float w = q.real.w;
float x = q.real.x;
float y = q.real.y;
float z = q.real.z;
// Extract rotational information
M.m00 = w * w + x * x - y * y - z * z;
M.m10 = 2 * x * y + 2 * w * z;
M.m20 = 2 * x * z - 2 * w * y;
M.m01 = 2 * x * y - 2 * w * z;
M.m11 = w * w + y * y - x * x - z * z;
M.m21 = 2 * y * z + 2 * w * x;
M.m02 = 2 * x * z + 2 * w * y;
M.m12 = 2 * y * z - 2 * w * x;
M.m22 = w * w + z * z - x * x - y * y;
// Extract translation information
Quaternion t = q.dual.Scale(2.0f) * q.real.Conjugate();
M.m03 = t.x;
M.m13 = t.y;
M.m23 = t.z;
return(M);
}
private DualQuaternion GetObjectCenteredRotationTransform(ChannelOutputs outputs, ScalingTransform parentScale)
{
DualQuaternion localSpaceTransform = GetJointCenteredRotationTransform(outputs, parentScale.Scale);
Vector3 centerPoint = CenterPoint.GetValue(outputs);
centerPoint = parentScale.Transform(centerPoint);
return(DualQuaternion.FromTranslation(-centerPoint).Chain(localSpaceTransform).Chain(DualQuaternion.FromTranslation(+centerPoint)));
}
public static DualQuaternion Normalize(DualQuaternion dq)
{
float len = Mathf.Sqrt(dq.real.x * dq.real.x + dq.real.y * dq.real.y + dq.real.z * dq.real.z + dq.real.w * dq.real.w);
dq.real = dq.real.Multiply(1 / len);
dq.dual = dq.dual.Multiply(1 / len);
return(dq);
}
public static DualQuaternion fromUnityTransform(Quaternion rot, Vector3 pos)
{
var r = new DualQuaternion(rot.conjugate(), Vector3.zero);
var t = new DualQuaternion(Quaternion.identity, pos);
var result = t * r;
return(result);
}
private DualQuaternion GetJointCenteredRotationTransform(ChannelOutputs outputs, Matrix3x3 parentScale)
{
Quaternion worldSpaceRotation = GetRotation(outputs);
Vector3 translation = Vector3.Transform(Translation.GetValue(outputs), parentScale);
return(DualQuaternion.FromRotationTranslation(worldSpaceRotation, translation));
}
public StagedSkinningTransform GetChainedTransform(ChannelOutputs outputs, StagedSkinningTransform parentTransform)
{
ScalingTransform scalingTransform = GetObjectCenteredScalingTransform(outputs);
ScalingTransform chainedScalingTransform = scalingTransform.Chain(parentTransform.ScalingStage);
DualQuaternion rotationTransform = GetObjectCenteredRotationTransform(outputs, parentTransform.ScalingStage);
DualQuaternion chainedRotationTransform = rotationTransform.Chain(parentTransform.RotationStage);
return(new StagedSkinningTransform(chainedScalingTransform, chainedRotationTransform));
}
public void Add(float weight, DualQuaternion dq)
{
if (Quaternion.Dot(realAccumulator, dq.Real) < 0)
{
weight *= -1;
}
realAccumulator += weight * dq.Real;
dualAccumulator += weight * dq.Dual;
}
public void TestFromAndToRotationTranslation()
{
Quaternion rotation = Quaternion.RotationYawPitchRoll(1, 2, 3);
Vector3 translation = new Vector3(2, 3, 4);
DualQuaternion dq = DualQuaternion.FromRotationTranslation(rotation, translation);
Assert.AreEqual(0, Vector3.Distance(translation, dq.Translation), 1e-6);
Assert.IsTrue(dq.Rotation == rotation || dq.Rotation == -rotation);
}
void Start()
{
transform.position = Targets.position;
transform.rotation = Targets.rotation;
dualQuaternions.Add(new DualQuaternion(transform.position, transform.rotation));
foreach (Transform tf in Targets)
{
var dq = new DualQuaternion(tf.position, tf.rotation);
dualQuaternions.Add(dq);
}
}
public override bool Equals(object obj)
{
if (obj is DualQuaternion)
{
DualQuaternion dq = (DualQuaternion)obj;
return(dq == this);
}
else
{
return(false);
}
}
public void TestFromMatrixRobustness()
{
Quaternion rotation = Quaternion.RotationYawPitchRoll(1, 2, 3);
Vector3 translation = new Vector3(2, 3, 4);
Vector3 scale = new Vector3(3, 4, 5);
Matrix matrix = Matrix.Scaling(scale) * Matrix.RotationQuaternion(rotation) * Matrix.Translation(translation);
DualQuaternion dq = DualQuaternion.FromMatrix(matrix);
Assert.AreEqual(0, Vector3.Distance(translation, dq.Translation), 1e-6);
Assert.IsTrue(dq.Rotation == rotation || dq.Rotation == -rotation);
}
public static DualQuaternion Inverse(DualQuaternion dq)
{
float real = (dq.real.w * dq.real.w + dq.real.x * dq.real.x + dq.real.y * dq.real.y + dq.real.z * dq.real.z),
dual = (dq.real.w * dq.dual.w + dq.real.x * dq.dual.x + dq.real.y * dq.dual.y + dq.real.z * dq.dual.z) * 2.0f;
DualQuaternion other;
other.real = Quaternion.Inverse(dq.real);
other.dual = new Quaternion(dq.dual.x * (dual - real), dq.dual.y * (dual - real), dq.dual.z * (dual - real), dq.dual.w * (real - dual));
return(other.normalize);
}
// Recursively update all bone transforms
static void FetchBoneMatrices(Transform bone, DualQuaternion parentTransform)
{
if (allBones.TryGetValue(bone.name, out int idx))
{
boneTransforms[idx] = parentTransform * (new DualQuaternion(bone));
foreach (Transform childBone in bone)
{
FetchBoneMatrices(childBone, boneTransforms[idx]);
}
}
}
public void TestTransform()
{
Vector3 translation = new Vector3(2, 3, 4);
Quaternion rotation = Quaternion.RotationYawPitchRoll(1, 2, 3);
Matrix matrix = Matrix.RotationQuaternion(rotation) * Matrix.Translation(translation);
DualQuaternion dq = DualQuaternion.FromRotationTranslation(rotation, translation);
Vector3 v = new Vector3(3, 4, 5);
Assert.IsTrue(dq.Transform(v) == Vector3.TransformCoordinate(v, matrix));
}
public void Apply(ChannelInputs inputs, Pose pose, DualQuaternion rootTransform)
{
foreach (Bone bone in boneSystem.Bones)
{
bone.AddRotation(orientationOutputs, inputs, pose.BoneRotations[bone.Index]);
}
var rescaledRootTransform = DualQuaternion.FromRotationTranslation(rootTransform.Rotation, rootTransform.Translation * 100);
boneSystem.RootBone.SetRotation(orientationOutputs, inputs, rescaledRootTransform.Rotation);
boneSystem.RootBone.SetTranslation(inputs, rescaledRootTransform.Translation);
boneSystem.Bones[1].SetTranslation(inputs, pose.RootTranslation);
}
public void TestInvert()
{
Vector3 translation = new Vector3(2, 3, 4);
Quaternion rotation = Quaternion.RotationYawPitchRoll(1, 2, 3);
DualQuaternion dq = DualQuaternion.FromRotationTranslation(rotation, translation);
Vector3 v = new Vector3(3, 4, 5);
Vector3 transformed = dq.Transform(v);
Vector3 inverseTransformed = dq.Invert().Transform(transformed);
Assert.AreEqual(
v,
inverseTransformed);
}
void drawDualQuaternion(DualQuaternion q)
{
Vector3 x = Vector3.right * 5.0f;
Vector3 y = Vector3.up * 5.0f;
Vector3 z = Vector3.forward * 5.0f;
Vector3 p = Vector3.zero;
var x1 = q.transformPoint(x);
var y1 = q.transformPoint(y);
var z1 = q.transformPoint(z);
var p1 = q.transformPoint(p);
Gizmos.DrawLine(p1, x1);
Gizmos.DrawLine(p1, y1);
Gizmos.DrawLine(p1, z1);
}
public void SimpleDualQuaternionTest2()
{
// Translation 5 units in X direction
var translation = DualQuaternion.CreateTranslation(5, Vector3.UnitX);
// Rotation of PI/2 around Z axis centered on origin - We are using Plücker coordinates to describe the axis of rotation
var rotation = DualQuaternion.CreateRotationPlucker(Math.PI / 2f, Vector3.UnitZ, Vector3.Zero);
// We compose displacements, translation is applied first
var displacement = rotation * translation;
// We apply the point transform (Clifford conjugation for points) on the origin point using the displacement
var point = displacement.TransformPoint();
// Assert
Assert.That(point, Is.EqualTo(new Vector3(0, 5, 0)));
}
static void BakeFrame(uint globalFrameIndex, bool morton, int size, int weaponSize, Color[][] pixels, Color[][] weaponPixels)
{
// Store the bone animation to an array of textures
for (uint bone = 0; bone < allBones.Count; bone++)
{
DualQuaternion dq = boneTransforms[bone];
if (dq == null)
{
continue;
}
var row0 = dq.real.ToVector4();
var row1 = dq.dual.ToVector4();
uint z;
if (morton)
{
uint y = globalFrameIndex;
uint x = bone;
z = MathHelper.EncodeMorton(x, y);
}
else
{
uint y = globalFrameIndex % (uint)size;
uint x = bone + globalFrameIndex / (uint)size * (uint)allBones.Count;
z = y * (uint)size + x;
}
pixels[0][z] = row0;
pixels[1][z] = row1;
if (bone == weaponBoneID)
{
uint y = globalFrameIndex % (uint)weaponSize;
uint x = 0 + globalFrameIndex / (uint)weaponSize * 1;
z = y * (uint)weaponSize + x;
weaponPixels[0][z] = row0;
weaponPixels[1][z] = row1;
}
}
}
public ChannelInputs Update(ChannelInputs shapeInputs, FrameUpdateParameters updateParameters, ControlVertexInfo[] previousFrameControlVertexInfos)
{
ChannelInputs inputs = new ChannelInputs(shapeInputs);
for (int idx = 0; idx < inputs.RawValues.Length; ++idx)
{
inputs.RawValues[idx] += model.Inputs.RawValues[idx];
}
dragHandle.Update(updateParameters);
DualQuaternion rootTransform = DualQuaternion.FromMatrix(dragHandle.Transform);
var blendedPose = GetBlendedPose(updateParameters.Time);
poser.Apply(inputs, blendedPose, rootTransform);
ikAnimator.Update(updateParameters, inputs, previousFrameControlVertexInfos);
proceduralAnimator.Update(updateParameters, inputs);
return(inputs);
}
void animate()
{
// if (updateMatrices) {
// heolienne.UpdateModelsMat ();
// updateMatrices = false;
// gridCacheIsUpToDate = false;
// }
DualQuaternion dq = new DualQuaternion(Quaternion.FromEulerAngles(0f, heolAngle, 0f), new Vector3(0f, 0f, 0f));
// for (int i = 0; i < heoliennes.InstancedDatas.Length; i++) {
// heoliennes.InstancedDatas [i].quat0 = new DualQuaternion(Quaternion.FromEulerAngles(heolAngle*0.2f,0f,0f),new Vector3(0f,0f,0f)).m_real;
// heoliennes.InstancedDatas [i].bpos1 = dq.m_dual;
// heoliennes.InstancedDatas [i].quat1 = heoliennes.InstancedDatas [i].quat0*dq.m_real;
// }
// heoliennes.UpdateInstancesData ();
for (int i = 0; i < heollow.InstancedDatas.Length; i++)
{
//heollow.InstancedDatas [i].bpos1 = dq.m_dual;
heollow.InstancedDatas [i].quat1 = dq.m_real;
}
heollow.UpdateInstancesData();
// for (int i = 0; i < pawn.InstancedDatas.Length; i++) {
// pawn.InstancedDatas [i].quat1 = Quaternion.FromEulerAngles(pawnAngle,pawnAngle,pawnAngle);
// }
// pawn.UpdateInstancesData ();
//
// pawnAngle += pawnAngleIncrement;
//
// if (pawnAngleIncrement > 0f){
// if (pawnAngle > pawnAngleLimit)
// pawnAngleIncrement = -pawnAngleIncrement;
// }else if (pawnAngle < -pawnAngleLimit)
// pawnAngleIncrement = -pawnAngleIncrement;
heolAngle += MathHelper.Pi * 0.007f;
}
public static DualQuaternion sclerp(DualQuaternion from, DualQuaternion to, float t)
{
float dot = from.dot(to);
if (dot < 0)
{
to = to * -1.0f;
}
var diff = from.conjugate() * to;
var vr = diff.real.getVectorPart();
var vd = diff.dual.getVectorPart();
float invr = 1.0f / Mathf.Sqrt(Vector3.Dot(vr, vr));
float angle = 2.0f * Mathf.Acos(diff.real.w);
float pitch = -2.0f * diff.dual.w * invr;
Vector3 dir = vr * invr;
Vector3 moment = (vd - dir * pitch * diff.real.w * 0.5f) * invr;
angle *= t;
pitch *= t;
float sinAngle = Mathf.Sin(0.5f * angle);
float cosAngle = Mathf.Cos(0.5f * angle);
var result = new DualQuaternion();
result.real = new Quaternion(dir.x * sinAngle, dir.y * sinAngle, dir.z * sinAngle, cosAngle);
var rdv = (sinAngle * moment + pitch * 0.5f * cosAngle * dir);
result.dual = new Quaternion(rdv.x, rdv.y, rdv.z, -pitch * 0.5f * sinAngle);
return(result);
}
void animate()
{
// if (updateMatrices) {
// heolienne.UpdateModelsMat ();
// updateMatrices = false;
// gridCacheIsUpToDate = false;
// }
DualQuaternion dq = new DualQuaternion(Quaternion.FromEulerAngles(0f,heolAngle,0f),new Vector3(0f,0f,0f));
// for (int i = 0; i < heoliennes.InstancedDatas.Length; i++) {
// heoliennes.InstancedDatas [i].quat0 = new DualQuaternion(Quaternion.FromEulerAngles(heolAngle*0.2f,0f,0f),new Vector3(0f,0f,0f)).m_real;
// heoliennes.InstancedDatas [i].bpos1 = dq.m_dual;
// heoliennes.InstancedDatas [i].quat1 = heoliennes.InstancedDatas [i].quat0*dq.m_real;
// }
// heoliennes.UpdateInstancesData ();
for (int i = 0; i < heollow.InstancedDatas.Length; i++) {
//heollow.InstancedDatas [i].bpos1 = dq.m_dual;
heollow.InstancedDatas [i].quat1 = dq.m_real;
}
heollow.UpdateInstancesData ();
// for (int i = 0; i < pawn.InstancedDatas.Length; i++) {
// pawn.InstancedDatas [i].quat1 = Quaternion.FromEulerAngles(pawnAngle,pawnAngle,pawnAngle);
// }
// pawn.UpdateInstancesData ();
//
// pawnAngle += pawnAngleIncrement;
//
// if (pawnAngleIncrement > 0f){
// if (pawnAngle > pawnAngleLimit)
// pawnAngleIncrement = -pawnAngleIncrement;
// }else if (pawnAngle < -pawnAngleLimit)
// pawnAngleIncrement = -pawnAngleIncrement;
heolAngle += MathHelper.Pi * 0.007f;
}
