92 lines
3.4 KiB
C#
92 lines
3.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using static System.Math;
|
|
|
|
public static class CableJointsAlgorithm {
|
|
public static void TimeStep(List<Cable> cables) {
|
|
//handle each cable individually
|
|
foreach (var cable in cables) {
|
|
//calculate new attachment points for all joints
|
|
for(var go = cable.firstRoller; go != null; go = go.GetComponent<RollerProperties>().linkTo) {
|
|
if (go.GetComponent<RollerProperties>().linkTo != null) {
|
|
var dist = go.GetComponent<DistanceJoint2D>();
|
|
var rp = go.GetComponent<RollerProperties>();
|
|
var (left, right) = TangentCircleCircle(go, rp.linkTo);
|
|
|
|
//TODO: assumes circles are flat for sufficiently small timesteps
|
|
var leftSurfaceDist =
|
|
(go.transform.TransformPoint(dist.anchor) - go.transform.TransformPoint(left)).magnitude;
|
|
var rightSurfaceDist = (dist.attachedRigidbody.transform.TransformPoint(dist.connectedAnchor) -
|
|
dist.attachedRigidbody.transform.TransformPoint(right)).magnitude;
|
|
|
|
Debug.Log(go.name+": left dist: "+leftSurfaceDist+" right Dist: "+rightSurfaceDist);
|
|
|
|
dist.anchor = left;
|
|
dist.connectedAnchor = right;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
public static (Vector2, Vector2) TangentCircleCircle(GameObject g1, GameObject g2) {
|
|
var roller1 = g1.GetComponent<RollerProperties>();
|
|
var roller2 = g2.GetComponent<RollerProperties>();
|
|
var t1 = g1.transform;
|
|
var t2 = g2.transform;
|
|
|
|
//assume round rollers only
|
|
var r1 = t1.lossyScale.x/2;
|
|
var r2 = t2.lossyScale.x/2;
|
|
|
|
Vector2 d = t2.Position2d() - t1.Position2d();
|
|
float r = r1 + r2;
|
|
if (roller1.clockwise == roller2.clockwise) {
|
|
r = r2 - r1;
|
|
}
|
|
|
|
var dLen = d.magnitude;
|
|
if (dLen > r) {
|
|
float alpha;
|
|
if (d.x >= 0) {
|
|
alpha = (float)Asin(d.y / dLen);
|
|
}
|
|
else {
|
|
alpha = (float)(PI - Asin(d.y / dLen));
|
|
}
|
|
|
|
float phi = (float)Asin(r / dLen); //TODO verify, paper says "|c|"
|
|
|
|
float alpha1, alpha2;
|
|
if (roller1.clockwise == roller2.clockwise) {
|
|
if (roller1.clockwise) {
|
|
alpha1 = (float)(alpha - PI / 2 - phi);
|
|
alpha2 = (float)(alpha - PI / 2 + phi);
|
|
}
|
|
else {
|
|
alpha1 = (float)(alpha + PI / 2 + phi);
|
|
alpha2 = (float)(alpha + PI / 2 + phi);
|
|
}
|
|
}
|
|
else {
|
|
if (roller1.clockwise) {
|
|
alpha1 = (float)(alpha - PI / 2 + phi);
|
|
alpha2 = (float)(alpha + PI / 2 + phi);
|
|
}
|
|
else {
|
|
alpha1 = (float)(alpha + PI / 2 - phi);
|
|
alpha2 = (float)(alpha - PI / 2 - phi);
|
|
}
|
|
}
|
|
|
|
var p1 = t1.Position2d() + r1 * new Vector2((float)Cos(alpha1), (float)Sin(alpha1));
|
|
var p2 = t2.Position2d() + r2 * new Vector2((float)Cos(alpha2), (float)Sin(alpha2));
|
|
|
|
return (t1.InverseTransformPoint(p1), t2.InverseTransformPoint(p2));
|
|
|
|
}
|
|
|
|
throw new Exception("Overlapping Rollers Error");
|
|
}
|
|
} |