Skip to content

Commit

Permalink
Added inverse kinematic
Browse files Browse the repository at this point in the history
  • Loading branch information
Tornado-Technology committed Aug 1, 2024
1 parent ffc36f4 commit 0ab9466
Show file tree
Hide file tree
Showing 3 changed files with 147 additions and 0 deletions.
6 changes: 6 additions & 0 deletions Hypercube.Math/Vectors/Vector2.cs
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,12 @@ public Vector2 Normalized
[MethodImpl(MethodImplOptions.AggressiveInlining)]
get => this / Length;
}

public float Angle
{
[MethodImpl(MethodImplOptions.AggressiveInlining)]
get => MathF.Atan2(Y, X);
}

public Vector2(float x, float y)
{
Expand Down
96 changes: 96 additions & 0 deletions Hypercube.Shared/Animation/Procedural/IKFabric2D.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
using Hypercube.Math.Vectors;

namespace Hypercube.Shared.Animation.Procedural;

public sealed class IKFabric2D
{
public Vector2 Position { get; private set; }
public Vector2 Target { get; private set; }
public bool Fixed { get; private set; }
public float MaxReach { get; private set; }

private readonly List<Segment2D> _segments;

public IKFabric2D(Vector2 position, int segments, float angle, float segmentLength, bool @fixed = true)
{
Position = position;
Fixed = @fixed;

_segments = new List<Segment2D>
{
new(position, angle, segmentLength)
};

MaxReach += segmentLength;

for (var i = 1; i < segments; i++)
{
AddSegment(angle, segmentLength);
}
}

public bool CanReach(Vector2 target)
{
return (Position - target).LengthSquared <= MaxReach * MaxReach;
}

public void AddSegment(float angle, float length)
{
var previous = _segments[^1];

MaxReach += length;
var segment = new Segment2D(Vector2.Zero, angle, length);

_segments.Add(segment);

segment.Follow(previous.Position);
}

public void Update()
{
for (var i = 0; i < _segments.Count; i++)
{
var segment = _segments[i];
segment.Update();

if (i == 0) {
segment.Follow(Target);
continue;
}

var previous = _segments[i - 1];
segment.Follow(previous.Position);
}

var last = _segments.Count - 1;
var lastSegment = _segments[last];

if (Fixed)
lastSegment.SetPosition(Position);

lastSegment.Update();

for (var i = last - 1; i >= 0; i--) {
var segment = _segments[i];
var nextSegment = _segments[i + 1];

segment.SetPosition(nextSegment.TargetPosition);
segment.Update();
}
}

public void SetPosition(Vector3 position)
{
Position = position;
}

public void SetTarget(Vector3 target)
{
Target = target;
}

public void SetFixed(bool value)
{
Fixed = value;
}
}
45 changes: 45 additions & 0 deletions Hypercube.Shared/Animation/Procedural/Segment2D.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
using Hypercube.Math.Vectors;

namespace Hypercube.Shared.Animation.Procedural;

public sealed class Segment2D
{
public float Angle { get; private set; }
public float Length { get; private set; }

public Vector2 Position { get; private set; }
public Vector2 TargetPosition { get; private set; }

public Segment2D(Vector2 position, float angle, float length)
{
Position = position;
Angle = angle;
Length = length;

Update();
}

public void Follow(Vector2 target)
{
var direction = target - Position;

Angle = direction.Angle;

// set magnitude
direction = direction.Normalized * Length;
direction *= -1;

Position = target + direction;
}

public void Update()
{
var delta = new Vector2(MathF.Cos(Angle), MathF.Sin(Angle)) * Length;
TargetPosition = Position + delta;
}

public void SetPosition(Vector2 position)
{
Position = position;
}
}

0 comments on commit 0ab9466

Please sign in to comment.