94 lines
4.1 KiB
C#
94 lines
4.1 KiB
C#
// Copyright (c) Microsoft Corporation.
|
|
// Licensed under the MIT License.
|
|
|
|
using System.Runtime.CompilerServices;
|
|
using UnityEngine;
|
|
|
|
[assembly: InternalsVisibleTo("Microsoft.MixedReality.Toolkit.Tests.PlayModeTests")]
|
|
namespace Microsoft.MixedReality.Toolkit.Experimental.Physics
|
|
{
|
|
public class VolumeElasticSystem : IElasticSystem<Vector3>
|
|
{
|
|
// Internal system state.
|
|
private Vector3 currentValue;
|
|
private Vector3 currentVelocity;
|
|
|
|
// Configuration of extent and spring properties.
|
|
private VolumeElasticExtent extent;
|
|
private ElasticProperties elasticProperties;
|
|
|
|
/// <summary>
|
|
/// Default constructor; initializes the elastic system with the specified
|
|
/// initial value, velocity, extent, and elastic properties.
|
|
/// </summary>
|
|
public VolumeElasticSystem(Vector3 initialValue, Vector3 initialVelocity,
|
|
VolumeElasticExtent extentInfo,
|
|
ElasticProperties elasticProperties)
|
|
{
|
|
currentValue = initialValue;
|
|
currentVelocity = initialVelocity;
|
|
this.extent = extentInfo;
|
|
this.elasticProperties = elasticProperties;
|
|
}
|
|
|
|
/// <inheritdoc/>
|
|
public Vector3 ComputeIteration(Vector3 forcingValue, float deltaTime)
|
|
{
|
|
// F = -kx - (drag * v)
|
|
Vector3 force = (forcingValue - currentValue) * elasticProperties.HandK - elasticProperties.Drag * currentVelocity;
|
|
|
|
// Iterate over each snapping point, and apply forces as necessary.
|
|
foreach (Vector3 snapPoint in extent.SnapPoints)
|
|
{
|
|
Vector3 closestPoint = extent.RepeatSnapPoints ? FindNearest(currentValue, snapPoint) : snapPoint;
|
|
// Calculate distance from snapping point.
|
|
Vector3 distFromSnappingPoint = closestPoint - currentValue;
|
|
force += ComputeSnapForce(distFromSnappingPoint, elasticProperties.SnapK, extent.SnapRadius);
|
|
}
|
|
|
|
// Check if our current value is within the specified bounds.
|
|
// If outside, we will apply the end-force (if the bounds are enabled)
|
|
if (!extent.StretchBounds.Contains(currentValue) && extent.UseBounds)
|
|
{
|
|
Vector3 closestPoint = extent.StretchBounds.ClosestPoint(currentValue);
|
|
Vector3 displacementFromEdge = closestPoint - currentValue;
|
|
|
|
// Apply the force (F = kx)
|
|
force += displacementFromEdge * elasticProperties.EndK;
|
|
}
|
|
|
|
// a = F/m
|
|
Vector3 accel = force / elasticProperties.Mass;
|
|
|
|
// Integrate our acceleration over time.
|
|
currentVelocity += accel * deltaTime;
|
|
// Integrate our velocity over time.
|
|
currentValue += currentVelocity * deltaTime;
|
|
|
|
return currentValue;
|
|
}
|
|
|
|
public Vector3 GetCurrentValue() => currentValue;
|
|
public Vector3 GetCurrentVelocity() => currentVelocity;
|
|
|
|
// Find the nearest snapping point to the given value, on a repeated
|
|
// snapping interval.
|
|
private Vector3 FindNearest(Vector3 value, Vector3 interval)
|
|
{
|
|
Debug.Assert(interval != Vector3.zero, "Zero vector used for repeating snapping interval; divide by zero!");
|
|
return new Vector3(Mathf.Round(value.x / interval.x) * interval.x,
|
|
Mathf.Round(value.y / interval.y) * interval.y,
|
|
Mathf.Round(value.z / interval.z) * interval.z);
|
|
}
|
|
|
|
// Helper function to reduce force calculation copypasta.
|
|
private Vector3 ComputeSnapForce(Vector3 distFromPoint, float k, float radius)
|
|
{
|
|
// Snap force is calculated by multiplying the "-kx" factor by
|
|
// a clamped distance factor. This results in an overall
|
|
// hyperbolic profile to the force imparted by the snap point.
|
|
return (distFromPoint) * elasticProperties.SnapK * (1.0f - Mathf.Clamp01(Mathf.Abs(distFromPoint.magnitude / radius)));
|
|
}
|
|
}
|
|
}
|