/// <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());
}
}
public static SCNQuaternion CreateQuaternion(SCNVector3 v1, SCNVector3 v2)
{
var a = SCNVector3.Cross(v1, v2);
var w = (float)Math.Sqrt(v1.LengthSquared * v2.LengthSquared) + SCNVector3.Dot(v1, v2);
var result = new SCNQuaternion(a.X, a.Y, a.Z, w);
result.Normalize();
return(result);
}
public static FeatureHitTestResult?HitTestFromOrigin(this ARSCNView view, SCNVector3 origin, SCNVector3 direction)
{
FeatureHitTestResult?result = null;
ARPointCloud features = null;
using (var frame = view.Session.CurrentFrame)
{
features = frame?.RawFeaturePoints;
}
if (features != null)
{
var points = features.Points;
// Determine the point from the whole point cloud which is closest to the hit test ray.
var closestFeaturePoint = origin;
var minDistance = float.MaxValue; // Float.greatestFiniteMagnitude
for (nuint i = 0; i < features.Count; i++)
{
var feature = points[i];
var featurePosition = new SCNVector3((Vector3)feature);
var originVector = origin - featurePosition;
var crossProduct = SCNVector3.Cross(originVector, direction);
var featureDistanceFromResult = crossProduct.Length;
if (featureDistanceFromResult < minDistance)
{
closestFeaturePoint = featurePosition;
minDistance = featureDistanceFromResult;
}
}
// Compute the point along the ray that is closest to the selected feature.
var originToFeature = closestFeaturePoint - origin;
var hitTestResult = origin + (direction * SCNVector3.Dot(direction, originToFeature));
var hitTestResultDistance = (hitTestResult - origin).Length;
result = new FeatureHitTestResult
{
Position = hitTestResult,
DistanceToRayOrigin = hitTestResultDistance,
FeatureHit = closestFeaturePoint,
FeatureDistanceToHitResult = minDistance
};
}
return(result);
}
/// <summary>
/// Returns the interpolated transform at a given length (l from 0.0 to totalLength)
/// </summary>
public SCNMatrix4 Transform(float l)
{
var position = this.Position(l);
var x = this.Tangent(l);
var y = SCNVector3.Normalize(this.UpVector);
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);
var matrix = SCNMatrix4.Rotate(rot.ToQuaternion());
return(matrix.SetTranslation((OpenTK.Vector3)position));
}
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);
});
}
internal NMatrix4 DragPlaneTransform(SCNNode camera)
{
// Create a transform for a XZ-plane. This transform can be passed to unproject() to
// map the user's touch position in screen space onto that plane in 3D space.
// The plane's transform is constructed from a given normal.
var yVector = Direction.Normalized();
var xVector = SCNVector3.Cross(yVector, camera.WorldRight);
var zVector = SCNVector3.Cross(xVector, yVector).Normalized();
var asIfsimd4x4 = Utilities.NMatrix4Create(new[] {
Utilities.SCNVector4Create(xVector, 0),
Utilities.SCNVector4Create(yVector, 0),
Utilities.SCNVector4Create(zVector, 0),
Utilities.SCNVector4Create(Origin, 1)
});
// But we're not, so transpose
asIfsimd4x4.Transpose();
return(asIfsimd4x4);
}
internal NMatrix4 DragPlaneTransform(SCNVector3 cameraPos)
{
var camToRayOrigin = this.Origin - cameraPos;
camToRayOrigin.NormalizeFast();
// Create a transform for a XZ-plane. This transform can be passed to unproject() to
// map the user's touch position in screen space onto that plane in 3D space.
// The plane's transform is constructed such that:
// 1. The ray along which we want to drag the object is the plane's X axis.
// 2. The plane's Z axis is ortogonal to the X axis and orthogonal to the vector
// from the camera to the object.
//
// Defining the plane this way has two main benefits:
// 1. Since we want to drag the object along an axis (not on a plane), we need to
// do one more projection from the plane's 2D space to a 1D axis. Since the axis to
// drag on is the plane's X-axis, we can later simply convert the un-projected point
// into the plane's local coordinate system and use the value on the X axis.
// 2. The plane's Z-axis is chosen to maximize the plane's coverage of screen space.
// The unprojectPoint() method will stop returning positions if the user drags their
// finger on the screen across the plane's horizon, leading to a bad user experience.
// So the ideal plane is parallel or almost parallel to the screen, but this is not
// possible when dragging along an axis which is pointing at the camera. For that case
// we try to find a plane which covers as much screen space as possible.
var xVector = Direction;
var zVector = SCNVector3.Cross(xVector, camToRayOrigin).Normalized();
var yVector = SCNVector3.Cross(xVector, zVector).Normalized();
var asIfSimd4x4 = Utilities.NMatrix4Create(new[] {
Utilities.SCNVector4Create(xVector, 0),
Utilities.SCNVector4Create(yVector, 0),
Utilities.SCNVector4Create(zVector, 0),
Utilities.SCNVector4Create(Origin, 1)
});
// But we're not, so transpose
asIfSimd4x4.Transpose();
return(asIfSimd4x4);
}
public void AnimateVortex()
{
if (this.Delegate == null)
{
throw new Exception("No delegate");
}
if (this.vortexCylinder == null)
{
throw new Exception("Vortex animation cylinder not set");
}
// Vortex shape from animation
var vortexShape = this.vortexCylinder.PresentationNode.Scale;
var vortexHeightDelta = vortexShape.Y - this.lastVortexHeight;
this.lastVortexHeight = vortexShape.Y;
var vortexCenterY = this.vortexCylinder.PresentationNode.WorldPosition.Y;
var vortexCenterYDelta = vortexCenterY - this.lastVortexCenterY;
this.lastVortexCenterY = vortexCenterY;
// Deform shape over time
var maxOuterRadius = vortexShape.X;
var maxInnerRadius = maxOuterRadius * 0.2f; // 20 % from experiment
var maxOuterRadiusDelta = maxOuterRadius - this.lastOuterRadius;
this.lastOuterRadius = maxOuterRadius;
// Orbital velocity
var currentFront = this.vortexCylinder.PresentationNode.WorldFront;
var orbitalMoveDelta = (currentFront - this.lastFront).Length * maxInnerRadius;
this.lastFront = currentFront;
var orbitalVelocityFactor = 5f;
var orbitalVelocity = (orbitalMoveDelta / (float)GameTime.DeltaTime) * orbitalVelocityFactor;
var topBound = vortexCenterY + vortexShape.Y * 0.5f;
var bottomBound = vortexCenterY - vortexShape.Y * 0.5f;
var blockObjects = this.Delegate.AllBlockObjects;
var up = SCNVector3.UnitY;
foreach (var block in blockObjects)
{
if (block.PhysicsNode?.PhysicsBody != null)
{
var position = block.PhysicsNode.PresentationNode.WorldPosition;
var positionWithoutY = new SCNVector3(position.X, 0f, position.Z);
var distanceFromCenter = positionWithoutY.Length;
var directionToCenter = -SCNVector3.Normalize(positionWithoutY);
// Adjust radius into curve
// Equation representing a half radius chord of circle equation
var normalizedY = DigitExtensions.Clamp(position.Y / topBound, 0f, 1f);
var radiusFactor = (float)Math.Sqrt(4f - 3f * normalizedY * normalizedY) - 1f;
radiusFactor = radiusFactor * 0.8f + 0.2f;
var innerRadius = maxInnerRadius * radiusFactor;
var outerRadius = maxOuterRadius * radiusFactor;
// Cap velocity
var maxVelocity = 30f;
if (block.PhysicsNode.PhysicsBody.Velocity.Length > maxVelocity)
{
block.PhysicsNode.PhysicsBody.Velocity = SCNVector3.Normalize(block.PhysicsNode.PhysicsBody.Velocity) * maxVelocity;
}
var force = SCNVector3.Zero;
// Stage specific manipulation
var vortexDirection = SCNVector3.Cross(directionToCenter, up);
var speedInVortexDirection = SCNVector3.Dot(block.PhysicsNode.PhysicsBody.Velocity, vortexDirection);
// Stable vortex pull
var pullForceMagnitude = (speedInVortexDirection * speedInVortexDirection) * (float)(block.PhysicsNode.PhysicsBody.Mass) / distanceFromCenter;
force += pullForceMagnitude * directionToCenter;
// Pull into outer radius
var radialInwardForceMagnitude = RadialSpringConstant * (float)Math.Max(0d, distanceFromCenter - outerRadius);
force += radialInwardForceMagnitude * directionToCenter;
// Pull away from inner radius
var radialOutwardForceMagnitude = RadialSpringConstant * (float)Math.Max(0d, innerRadius - distanceFromCenter);
force += -radialOutwardForceMagnitude * directionToCenter;
// Vortex velocity adjustment
if (distanceFromCenter > innerRadius)
{
var tangentForceMagnitude = TangentVelocitySpringContant * (speedInVortexDirection - orbitalVelocity);
force += -tangentForceMagnitude * vortexDirection * (0.5f + (float)(random.NextDouble() * 1d));
}
// Random forces/torque
force += force.Length * (float)((random.NextDouble() * 2d - 1d) * MaxRandomVortexForce) * up;
this.ApplyRandomTorque(block.PhysicsNode.PhysicsBody, MaxRandomVortexTorque);
// Top bound pull down
var topBoundForceMagnitude = RadialSpringConstant * (float)Math.Max(0d, position.Y - topBound);
force += topBoundForceMagnitude * -up;
// Bottom bound pull up
var bottomBoundForceMagnitude = RadialSpringConstant * (float)Math.Max(0d, bottomBound - position.Y);
force += bottomBoundForceMagnitude * up;
block.PhysicsNode.PhysicsBody.ApplyForce(force, false);
// Scale the vortex
// The higher position in the bound, more it should move upward to scale the vortex
var normalizedPositionInBoundY = DigitExtensions.Clamp((position.Y - bottomBound) / vortexShape.Y, 0f, 1f);
var heightMoveFactor = Math.Abs(normalizedPositionInBoundY - 0.5f);
var newPositionY = position.Y + vortexCenterYDelta + vortexHeightDelta * heightMoveFactor;
var positionXZ = new SCNVector3(position.X, 0f, position.Z);
var radialMoveFactor = DigitExtensions.Clamp(distanceFromCenter / outerRadius, 0f, 1f);
var newPositionXZ = positionXZ + maxOuterRadiusDelta * radialMoveFactor * -directionToCenter;
block.PhysicsNode.WorldPosition = new SCNVector3(newPositionXZ.X, newPositionY, newPositionXZ.Z);
block.PhysicsNode.WorldOrientation = block.PhysicsNode.PresentationNode.WorldOrientation;
block.PhysicsNode.PhysicsBody.ResetTransform();
}
}
}
public SlingShotPose ComputeInputPose()
{
// note the -1 here differs from other usage
var data = new SlingShotPose {
UpVector = -this.UpVector /* negated because the strap Y-axis points down */
};
var startBend = this.CurrentLengthL / this.CurrentTotalLength;
var endBend = 1f - this.CurrentLengthR / this.CurrentTotalLength;
var leatherOnStraights = this.OriginalLeatherLength - this.CurrentLengthOnBall;
var segmentAStart = 0f;
var segmentAEnd = this.CurrentLengthL - leatherOnStraights * 0.5f;
var segmentCStart = segmentAEnd + this.OriginalLeatherLength;
var segmentCEnd = this.CurrentTotalLength;
var originalLeatherRange = this.OriginalLeatherLength / this.OriginalTotalLength;
var currentLeatherRange = this.OriginalLeatherLength / this.CurrentTotalLength;
for (var i = 0; i < this.SimulatedTransformCount; i++)
{
var l = this.OriginalTotalLength * (float)i / (float)(this.SimulatedTransformCount - 1f);
var u = l / this.OriginalTotalLength;
// remap the u value depending on the material (rubber vs leather)
var isRubber = Math.Abs(0.5f - u) > originalLeatherRange * 0.5f;
if (isRubber)
{
if (u < 0.5f)
{
u = u / (0.5f - originalLeatherRange * 0.5f);
u = (segmentAStart + (segmentAEnd - segmentAStart) * u) / this.CurrentTotalLength;
}
else
{
u = 1f - (1f - u) / (0.5f - originalLeatherRange * 0.5f);
u = (segmentCStart + (segmentCEnd - segmentCStart) * u) / this.CurrentTotalLength;
}
}
else
{
u = (startBend + endBend) * 0.5f - (0.5f - u) * (currentLeatherRange / originalLeatherRange);
}
var p = SCNVector3.Zero;
var t = SCNVector3.UnitX;
if (u < startBend)
{
// left straight
var value = u / startBend;
p = SimdExtensions.Mix(this.FixturePositionL,
this.TangentPositionL,
new SCNVector3(value, value, value)); // left rubber band
t = SCNVector3.Normalize(this.TangentPositionL - this.FixturePositionL);
}
else if (u > endBend)
{
// right straight
var value = (1f - u) / (1f - endBend);
p = SimdExtensions.Mix(this.FixturePositionR,
this.TangentPositionR,
new SCNVector3(value, value, value)); // right rubber band
t = SCNVector3.Normalize(this.FixturePositionR - this.TangentPositionR);
}
else
{
// on the ball
var upv = this.UpVector;
var rot = SCNQuaternion.FromAxisAngle(upv, -this.BetaAngle * (u - startBend) / (endBend - startBend));
p = this.ballPosition + rot.Act(this.TangentPositionL - this.ballPosition);
t = SCNVector3.Cross(upv, SCNVector3.Normalize(this.ballPosition - p));
}
data.Positions.Add(p);
data.Tangents.Add(t);
data.Lengths.Add(l);
}
return(data);
}
public override void OnDidApplyConstraints(ISCNSceneRenderer renderer)
{
var frameSkips = 3;
if ((GameTime.FrameCount + this.Index) % frameSkips == 0)
{
if (this.PhysicsNode != null)
{
if (this.worldPositions.Count > (this.MaxTrailPositions / frameSkips))
{
this.RemoveVerticesPair();
}
var position = this.PhysicsNode.PresentationNode.WorldPosition;
SCNVector3 trailDir;
if (this.worldPositions.Any())
{
var previousPosition = this.worldPositions.LastOrDefault();
trailDir = position - previousPosition;
var lengthSquared = trailDir.LengthSquared;
if (lengthSquared < Epsilon)
{
this.RemoveVerticesPair();
this.UpdateColors();
var tempPositions = this.tempWorldPositions.Select(tempPosition => this.trailNode.PresentationNode.ConvertPositionFromNode(tempPosition, null)).ToList();
this.trailNode.PresentationNode.Geometry = this.CreateTrailMesh(tempPositions, this.colors);
return;
}
trailDir = SCNVector3.Normalize(trailDir);
}
else
{
trailDir = this.ObjectRootNode.WorldFront;
}
var right = SCNVector3.Cross(SCNVector3.UnitY, trailDir);
right = SCNVector3.Normalize(right);
var scale = 1f; //Float(i - 1) / worldPositions.count
var halfWidth = this.TrailHalfWidth;
if (this.TrailShouldNarrow)
{
halfWidth *= scale;
}
var u = position + right * halfWidth;
var v = position - right * halfWidth;
this.worldPositions.Add(position);
this.tempWorldPositions.Add(u);
this.tempWorldPositions.Add(v);
this.colors.Add(SCNVector4.Zero);
this.colors.Add(SCNVector4.Zero);
this.UpdateColors();
var localPositions = this.tempWorldPositions.Select(tempPosition => this.trailNode.PresentationNode.ConvertPositionFromNode(tempPosition, null)).ToList();
this.trailNode.PresentationNode.Geometry = this.CreateTrailMesh(localPositions, this.colors);
}
}
}
public static IList <FeatureHitTestResult> HitTestWithFeatures(this ARSCNView view,
CGPoint point,
float coneOpeningAngleInDegrees,
float minDistance = 0,
float maxDistance = float.MaxValue,
int maxResults = 1)
{
var results = new List <FeatureHitTestResult>();
ARPointCloud features = null;
using (var frame = view.Session.CurrentFrame)
{
features = frame?.RawFeaturePoints;
}
if (features != null)
{
var ray = view.HitTestRayFromScreenPosition(point);
if (ray.HasValue)
{
var maxAngleInDegrees = Math.Min(coneOpeningAngleInDegrees, 360f) / 2f;
var maxAngle = (maxAngleInDegrees / 180f) * Math.PI;
var points = features.Points;
for (nuint j = 0; j < features.Count; j++)
{
var feature = points[j];
var featurePosition = new SCNVector3((Vector3)feature);
var originToFeature = featurePosition - ray.Value.Origin;
var crossProduct = SCNVector3.Cross(originToFeature, ray.Value.Direction);
var featureDistanceFromResult = crossProduct.Length;
var hitTestResult = ray.Value.Origin + (ray.Value.Direction * SCNVector3.Dot(ray.Value.Direction, originToFeature));
var hitTestResultDistance = (hitTestResult - ray.Value.Origin).Length;
if (hitTestResultDistance < minDistance || hitTestResultDistance > maxDistance)
{
// Skip this feature - it is too close or too far away.
continue;
}
var originToFeatureNormalized = SCNVector3.Normalize(originToFeature);
var angleBetweenRayAndFeature = Math.Acos(SCNVector3.Dot(ray.Value.Direction, originToFeatureNormalized));
if (angleBetweenRayAndFeature > maxAngle)
{
// Skip this feature - is outside of the hit test cone.
continue;
}
// All tests passed: Add the hit against this feature to the results.
results.Add(new FeatureHitTestResult
{
Position = hitTestResult,
DistanceToRayOrigin = hitTestResultDistance,
FeatureHit = featurePosition,
FeatureDistanceToHitResult = featureDistanceFromResult
});
}
// Sort the results by feature distance to the ray.
results = results.OrderBy(result => result.DistanceToRayOrigin).ToList();
// Cap the list to maxResults.
var cappedResults = new List <FeatureHitTestResult>();
var i = 0;
while (i < maxResults && i < results.Count)
{
cappedResults.Add(results[i]);
i += 1;
}
results = cappedResults;
}
}
return(results);
}