/// <summary>
/// Aligns the rigid bodies by correcting their orienation
/// This should be called after the simulation step
/// </summary>
private void AlignBones()
{
// orient the bodies accordingly
for (var i = this.BoneInset; i < this.simulatedTransforms.Count - this.BoneInset; i++)
{
if (!this.simulatedTransforms[i].Dynamic)
{
continue;
}
var a = this.simulatedTransforms[i - 1].Position;
var b = this.simulatedTransforms[i + 1].Position;
// this is the upVector computed for each bone of the rest pose
var transform = this.restPoseSpace * this.restPoseTransforms[i]; // todo: direction of multiply?
var y = SimdExtensions.CreateQuaternion(transform).Act(new SCNVector3(0f, 1f, 0f));
var x = SCNVector3.Normalize(b - a);
var z = SCNVector3.Normalize(SCNVector3.Cross(x, y));
y = SCNVector3.Normalize(SCNVector3.Cross(z, x));
var rot = new OpenTK.Matrix3(x.X, y.X, z.X, x.Y, y.Y, z.Y, x.Z, y.Z, z.Z);
this.simulatedTransforms[i].Orientation = new SCNQuaternion(rot.ToQuaternion());
}
}
/// <summary>
/// Disables the simulation on the slingshot and sets the rigid bodies to be driven by the input pose
/// </summary>
public void EnableInputPose()
{
for (var i = 0; i < this.simulatedTransforms.Count; i++)
{
var l = this.OriginalTotalLength * (float)i / (float)(this.simulatedTransforms.Count - 1);
var transform = this.InputPoseTransform(l);
var position = new SCNVector3(transform.Column3.X, transform.Column3.Y, transform.Column3.Z);
this.simulatedTransforms[i].Position = position;
this.simulatedTransforms[i].Orientation = SimdExtensions.CreateQuaternion(transform);
this.simulatedTransforms[i].Velocity = SCNVector3.Zero;
this.simulatedTransforms[i].Dynamic = false;
}
}
private void InitializeHelpers()
{
this.computedInputPose = new ComputedValue <SlingShotPose>(() => this.ComputeInputPose());
this.computedTangentPositionR = new ComputedValue <SCNVector3>(() => this.TangentPosition(this.FixturePositionR));
this.computedTangentPositionL = new ComputedValue <SCNVector3>(() => this.TangentPosition(this.FixturePositionL));
this.computedBetaAngle = new ComputedValue <float>(() =>
{
var d = SCNVector3.Normalize(this.ballPosition - this.CenterPosition);
var t = SCNVector3.Normalize(this.TangentPositionL - this.ballPosition);
var quaternion = SimdExtensions.CreateQuaternion(d, t);
quaternion.ToAxisAngle(out SCNVector3 _, out float angle);
return(2f * angle);
});
this.computedCenterPosition = new ComputedValue <SCNVector3>(() =>
{
var direction = SCNVector3.Cross(this.UpVector, this.TangentPositionR - this.TangentPositionL);
return(this.ballPosition - SCNVector3.Normalize(direction) * 1.25f * this.ballRadius);
});
this.computedRestPose = new ComputedValue <SlingShotPose>(() =>
{
var data = new SlingShotPose();
for (var i = 0; i < this.restPoseTransforms.Count; i++)
{
var transform = this.restPoseSpace * this.restPoseTransforms[i];
var p = new SCNVector3(transform.Column3.X, transform.Column3.Y, transform.Column3.Z);
var t = SimdExtensions.CreateQuaternion(transform).Act(new SCNVector3(1f, 0f, 0f));
var l = 0f;
if (i > 0)
{
l = data.Lengths[i - 1] + (p - data.Positions[i - 1]).Length;
}
data.Positions.Add(p);
data.Tangents.Add(t);
data.Lengths.Add(l);
}
return(data);
});
}
private void OrientToPlane(ARPlaneAnchor planeAnchor, ARCamera camera)
{
// Get board rotation about y
this.Orientation = SimdExtensions.CreateQuaternion(planeAnchor.Transform.ToSCNMatrix4());
var boardAngle = this.EulerAngles.Y;
// If plane is longer than deep, rotate 90 degrees
if (planeAnchor.Extent.X > planeAnchor.Extent.Z)
{
boardAngle += (float)Math.PI / 2f;
}
// Normalize angle to closest 180 degrees to camera angle
boardAngle = boardAngle.NormalizedAngle(camera.EulerAngles.Y, (float)Math.PI);
this.Rotate(boardAngle);
}
