public void TestGetAngle180(float[] a, float[] b, float expected)
{
var from = new Vector3(a[0], a[1], a[2]);
var to = new Vector3(b[0], b[1], b[2]);
var angle = VectorMath3D.GetAngle(from, to, Vector3.up);
Assert.AreEqual(expected, angle);
}
public static ILoftPath Create(Vector3 center, Vector3 start, Vector3 end, bool ccw)
{
var startVector = start - center;
var endVector = end - center;
var normal = Vector3.Cross(startVector, endVector).normalized;
if (normal.sqrMagnitude < 0.0001f) // 180 deg most likely
{
normal = Vector3.up;
}
if (Vector3.Dot(normal, Vector3.up) < 0)
{
normal = -normal;
}
var angle = VectorMath3D.GetAngle(startVector, endVector, normal);
if (angle < 0)
{
angle = Mathf.PI * 2 + angle;
}
if (ccw)
{
angle = -Mathf.PI * 2 + angle;
}
//for (float i = 0; i < Mathf.Abs(angle); i += .1f)
//{
// var n = Quaternion.AngleAxis(Mathf.Rad2Deg * i * Mathf.Sign(angle), normal) * startVector;
// Debug.DrawLine(center, center + n, angle > 0 ? Color.green : Color.red);
//}
//Debug.DrawLine(center, center + normal, Color.red);
//Debug.DrawLine(center, start, Color.cyan);
//Debug.DrawLine(center, end, Color.blue);
if (Mathf.Abs(angle) < 0.000001f)
{
return(new LinearPath(start, end));
}
else
{
return(new ArcLoftPath(startVector, angle, center, normal, endVector));
}
}
public void SnapTo(Vector3 to, out Vector3 position, out float distance)
{
// TODO: not completly accurate
Vector3 point = VectorMath3D.GetPointOnCircle(normal, radius, to - center);
float newAngle = VectorMath3D.GetAngle(startPosition, point, normal);
var angle = angleDistance;
if (angle < 0)
{
newAngle = Mathf.Clamp(newAngle, angle, 0);
}
else
{
newAngle = Mathf.Clamp(newAngle, 0, angle);
}
var clamped = GetPosition(newAngle, normal, startPosition);// Quaternion.AngleAxis(newAngle * Mathf.Rad2Deg, normal) * (startPosition);
var mag1 = (clamped + center - to).sqrMagnitude;
var mag2 = (endPosition + center - to).sqrMagnitude;
if (mag1 > mag2)
{
distance = newAngle * radius * Mathf.Sign(angleDistance);//Mathf.Sqrt(mag2);
position = endPosition + center;
}
else
{
position = clamped + center;
distance = newAngle * radius * Mathf.Sign(angleDistance);//Mathf.Sqrt(mag1);
}
}
public bool GetOffsetToPoint(Vector3 point, out float offset)
{
// TODO: optimize
var plane = new Plane(normal, Center);
var ray = new Ray(point, Vector3.up);
float project;
plane.Raycast(ray, out project);
//Debug.DrawLine(point, ray.GetPoint(project), Color.green);
point = ray.GetPoint(project);
var angle = VectorMath3D.GetAngle(startPosition, point - center, normal);
offset = angle * radius;
var planarCenter = this.center + normal * Vector3.Dot(normal, startPosition);
//Debug.DrawLine(planarCenter, planarCenter + Vector3.up * Mathf.PI, Color.black);
//Debug.DrawLine(planarCenter, planarCenter + Vector3.up * angle, Color.white);
if (arcLength > 0)
{
if (offset >= 0 && offset <= arcLength)
{
return(true);
}
}
else
{
if (offset >= arcLength && offset <= 0)
{
return(true);
}
}
offset = float.NaN;
return(false);
}
public void SnapTo(Vector3 to, out Vector3 position, out float distance)
{
position = VectorMath3D.GetPointOnSegment(start, dir, length, to, out distance);
}