Skip to content

Commit

Permalink
Fix unnecessary method call (#55)
Browse files Browse the repository at this point in the history 
* Lint gradient visuals

* Lint Scene.cs + APFGradientVisuals.cs

* Clean InverseKinematics.cs

* Fix unnecessary method call
  • Loading branch information
@bjrtx
bjrtx authored Apr 26, 2024
1 parent f37b16d commit 0e973db
Show file tree
Hide file tree
Showing 5 changed files with 354 additions and 440 deletions.
17 changes: 2 additions & 15 deletions Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/Scene.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,7 @@ public Limb(Transform physicalLimb, List<Transform> virtualLimb) {
/// <param name="applyDampening">Whether to apply dampening in the Razzaque et al.'s Redirected walking hybrid technique.</param>
/// <param name="applySmoothing">Whether to apply smoothing in the Razzaque et al.'s Redirected walking hybrid technique.</param>
[Serializable]
public record Scene() {
// [Ignore] public Transform physicalHand;
// [Ignore] public Transform virtualHand;
public record Scene {
[SerializeField] public List<Limb> limbs;

[Ignore] public List<Transform> virtualLimbs => limbs.SelectMany(limb => limb.virtualLimb).ToList();
Expand Down Expand Up @@ -84,9 +82,7 @@ public List<Vector3> LimbRedirection {
get => limbs.ConvertAll(limb => limb.virtualLimb[0].position - limb.physicalLimb.position);
set {
foreach ((var limb, var v) in limbs.Zip(value)) {
// Debug.Log(limb.virtualLimb.First().position);
limb.virtualLimb.ForEach(vLimb => vLimb.position = limb.physicalLimb.position + v);
// Debug.Log(limb.virtualLimb.First().position);
}
}
}
Expand Down Expand Up @@ -165,15 +161,6 @@ public void CopyLimbTranslationAndRotation() => limbs.ForEach(limb => {
/// </summary>
public void RotateVirtualHeadY(float angle) => HeadToHeadRedirection = Quaternion.Euler(0f, angle, 0f) * HeadToHeadRedirection;

public List<Collider2D> GetAllObstaclesCollider() {
GameObject[] Obstacles = GameObject.FindGameObjectsWithTag("Obstacle");
List<Collider2D> ObstaclesColliders = new();

for (int i = 0; i < Obstacles.Length; i++) {
ObstaclesColliders.Add(Obstacles[i].GetComponent<Collider2D>());
}

return ObstaclesColliders;
}
public List<Collider2D> GetAllObstaclesCollider() => GameObject.FindGameObjectsWithTag("Obstacle").Select(v => v.GetComponent<Collider2D>()).ToList();
}
}
4 changes: 1 addition & 3 deletions ...c Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Redirection/World Redirection/Strategies.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,7 @@ public class APFP2R : WorldRedirectionStrategy {

public List<Collider> colliders;

public APFP2R() : base() {
colliders = GameObject.FindGameObjectsWithTag("Obstacle").Select(o => o.GetComponent<Collider>()).ToList();
}
public APFP2R() : base() => colliders = GameObject.FindGameObjectsWithTag("Obstacle").Select(o => o.GetComponent<Collider>()).ToList();

public override Vector3 SteerTo(Scene scene) => ComputeGradient(scene);

Expand Down
189 changes: 88 additions & 101 deletions Visuo-haptic Toolkit/Assets/Visuo-Haptic Toolkit/Scripts/Utils/InverseKinematics.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,112 +5,99 @@
using static VHToolkit.MathTools;

// Adapted from UnityEngine.U2D.IK
public static class CCD2D
{
public static bool Solve(Vector3 targetPosition, Vector3 forward, int solverLimit, float tolerance, float velocity, ref Vector3[] positions)
{
int step = 0;
float sqrTolerance = tolerance * tolerance;
while ((targetPosition - positions[^1]).sqrMagnitude > sqrTolerance && step++ < solverLimit)
{
DoIteration(targetPosition, forward, velocity, ref positions);
}
public static class CCD2D {
public static bool Solve(Vector3 targetPosition, Vector3 forward, int maxSteps, float tolerance, float velocity, ref Vector3[] positions) {
int step = 0;
float sqrTolerance = tolerance * tolerance;
bool hasConverged;
while (!(hasConverged = (targetPosition - positions[^1]).sqrMagnitude <= sqrTolerance) && step++ < maxSteps) {
DoIteration(targetPosition, forward, velocity, ref positions);
}
return hasConverged;
}

return step != 0;
}

private static void DoIteration(Vector3 targetPosition, Vector3 forward, float velocity, ref Vector3[] positions)
{
for (int num = positions.Length - 2; num >= 0; num--)
{
float b = Vector3.SignedAngle(positions[^1] - positions[num], targetPosition - positions[num], forward);
b = Mathf.Lerp(0f, b, velocity);
Quaternion rotation = Quaternion.AngleAxis(b, forward);
for (int num2 = positions.Length - 2; num2 > num; num2--)
{
positions[num2] = positions[num] + rotation * (positions[num2] - positions[num]);
}
}
}
private static void DoIteration(Vector3 targetPosition, Vector3 forward, float velocity, ref Vector3[] positions) {
for (int num = positions.Length - 2; num >= 0; num--) {
float b = Vector3.SignedAngle(positions[^1] - positions[num], targetPosition - positions[num], forward);
b = Mathf.Lerp(0f, b, velocity);
Quaternion rotation = Quaternion.AngleAxis(b, forward);
for (int num2 = positions.Length - 2; num2 > num; num2--) {
positions[num2] = positions[num] + rotation * (positions[num2] - positions[num]);
}
}
}
}

// Adapted from UnityEngine.U2D.IK
public static class MyFABRIK2D
{
public static bool SolveWithFloatingOrigin(Vector2 targetPosition, int solverLimit, float tolerance, float[] lengths, ref Vector2[] positions)
{
int step = 0;
float sqrTolerance = tolerance * tolerance;
while ((targetPosition - positions[^1]).sqrMagnitude > sqrTolerance && step++ < solverLimit)
{
Forward(targetPosition, lengths, ref positions);
Backward(positions[0], lengths, ref positions);
}
return step != 0;
}
public static class MyFABRIK2D {
public static bool SolveWithFloatingOrigin(Vector2 targetPosition, int maxSteps, float tolerance, float[] lengths, ref Vector2[] positions) {
int step = 0;
float sqrTolerance = tolerance * tolerance;
bool hasConverged;
while (!(hasConverged = (targetPosition - positions[^1]).sqrMagnitude <= sqrTolerance) && step++ < maxSteps) {
Forward(targetPosition, lengths, ref positions);
Backward(positions[0], lengths, ref positions);
}
return hasConverged;
}

public static bool SolveWithFixedOrigin(Vector2 originPosition, Vector2 targetPosition, int maxSteps, float tolerance, float[] lengths, ref Vector2[] positions) {
Debug.Assert(lengths.Sum() >= Vector2.Distance(originPosition, targetPosition));
int step = 0;
float sqrTolerance = tolerance * tolerance;
bool hasConverged;
while (!(hasConverged = (targetPosition - positions[^1]).sqrMagnitude <= sqrTolerance) && step++ < maxSteps) {
Forward(targetPosition, lengths, ref positions);
Backward(originPosition, lengths, ref positions);
}
return hasConverged;
}

private static void Forward(Vector2 targetPosition, IList<float> lengths, ref Vector2[] positions) {
positions[^1] = targetPosition;
for (int num2 = positions.Length - 2; num2 >= 0; num2--) {
float num3 = lengths[num2] / (positions[num2 + 1] - positions[num2]).magnitude;
positions[num2] = Vector2.LerpUnclamped(positions[num2 + 1], positions[num2], num3);
}
}

public static bool SolveWithFixedOrigin(Vector2 originPosition, Vector2 targetPosition, int solverLimit, float tolerance, float[] lengths, ref Vector2[] positions)
{
Debug.Assert(lengths.Sum() >= Vector2.Distance(originPosition, targetPosition));
int step = 0;
float sqrTolerance = tolerance * tolerance;
while ((targetPosition - positions[^1]).sqrMagnitude > sqrTolerance && step++ < solverLimit)
{
Forward(targetPosition, lengths, ref positions);
Backward(originPosition, lengths, ref positions);
}
return step != 0;
}

private static void Forward(Vector2 targetPosition, IList<float> lengths, ref Vector2[] positions)
{
positions[^1] = targetPosition;
for (int num2 = positions.Length - 2; num2 >= 0; num2--)
{
float num3 = lengths[num2] / (positions[num2 + 1] - positions[num2]).magnitude;
positions[num2] = Vector2.LerpUnclamped(positions[num2 + 1], positions[num2], num3);
}
}
private static void Backward(Vector2 originPosition, IList<float> lengths, ref Vector2[] positions) {
positions[0] = originPosition;
int num = positions.Length - 1;
for (int i = 0; i < num; i++) {
float num2 = lengths[i] / (positions[i + 1] - positions[i]).magnitude;
positions[i + 1] = Vector2.LerpUnclamped(positions[i], positions[i + 1], num2);
}
}

private static void Backward(Vector2 originPosition, IList<float> lengths, ref Vector2[] positions)
{
positions[0] = originPosition;
int num = positions.Length - 1;
for (int i = 0; i < num; i++)
{
float num2 = lengths[i] / (positions[i + 1] - positions[i]).magnitude;
positions[i + 1] = Vector2.LerpUnclamped(positions[i], positions[i + 1], num2);
}
}
// Own functions:
static public IList<Vector2> InverseKinematics(Vector2 origin, Vector2 end, IList<float> lengths) {
// TODO figure out sensible solver limit and tolerance
bool wasSolved = false;
int solverLimit = 1;
float tolerance = Vector2.kEpsilon;
Vector2[] positions = new Vector2[lengths.Count - 1];
while (!wasSolved) {
wasSolved = SolveWithFixedOrigin(origin, end, solverLimit, tolerance, lengths.ToArray(), ref positions);
solverLimit *= 2;
tolerance *= 2;
}
return positions.ToList();
}

// Own functions:
static public IList<Vector2> InverseKinematics(Vector2 origin, Vector2 end, IList<float> lengths) {
// TODO figure out sensible solver limit and tolerance
bool wasSolved = false;
int solverLimit = 1;
float tolerance = Vector2.kEpsilon;
Vector2[] positions = new Vector2[lengths.Count - 1];
while (!wasSolved) {
wasSolved = SolveWithFixedOrigin(origin, end, solverLimit, tolerance, lengths.ToArray(), ref positions);
solverLimit *= 2;
tolerance *= 2;
}
return positions.ToList();
}

// Compute the forward kinematics chain from link-to-link length and angle information.
static public IList<PositionAndRotation2D> ForwardKinematics(PositionAndRotation2D origin, IList<float> lengths, IList<float> angles) {
Debug.Assert(lengths.Count == angles.Count);
List<PositionAndRotation2D> result = new() { origin };
var position = origin.position;
var direction = origin.forward.LiftFromHorizontalPlane();
foreach (var (length, angle) in lengths.Zip(angles)) {
Debug.Assert(length > 0);
Debug.Assert(Mathf.Abs(angle) <= 180f);
position += length * result.Last().forward;
direction = Quaternion.AngleAxis(angle, Vector3.up) * direction;
result.Add(new(position, direction.ProjectToHorizontalPlane()));
}
return result;
}
// Compute the forward kinematics chain from link-to-link length and angle information.
static public IList<PositionAndRotation2D> ForwardKinematics(PositionAndRotation2D origin, IList<float> lengths, IList<float> angles) {
Debug.Assert(lengths.Count == angles.Count);
List<PositionAndRotation2D> result = new() { origin };
var position = origin.position;
var direction = origin.forward.LiftFromHorizontalPlane();
foreach (var (length, angle) in lengths.Zip(angles)) {
Debug.Assert(length > 0);
Debug.Assert(Mathf.Abs(angle) <= 180f);
position += length * result.Last().forward;
direction = Quaternion.AngleAxis(angle, Vector3.up) * direction;
result.Add(new(position, direction.ProjectToHorizontalPlane()));
}
return result;
}
}
Loading

0 comments on commit 0e973db

Please sign in to comment.