//
// Tangent
//
public static MyVector3 GetTangent(MyVector3 posA, MyVector3 posB, MyVector3 handleA, MyVector3 handleB, float t)
{
//The tangent is also the derivative vector
MyVector3 tangent = MyVector3.Normalize(GetDerivativeVec(posA, posB, handleA, handleB, t));
return(tangent);
}
//Calculate the angle between two vectors
//This angle should be measured in 360 degrees (Vector3.Angle is measured in 180 degrees)
//Should maybe be moved to _Geometry??
//In 3d space [radians]
//https://stackoverflow.com/questions/5188561/signed-angle-between-two-3d-vectors-with-same-origin-within-the-same-plane
//https://math.stackexchange.com/questions/2906314/how-to-calculate-angle-between-two-vectors-in-3d-with-clockwise-or-counter-clock
public static float AngleFromToCCW(MyVector3 from, MyVector3 to, MyVector3 upRef)
{
//This is only working in 2d space
//float angleDegrees = Quaternion.FromToRotation(to.ToVector3(), from.ToVector3()).eulerAngles.y;
from = MyVector3.Normalize(from);
to = MyVector3.Normalize(to);
upRef = MyVector3.Normalize(upRef);
float angleRad = AngleBetween(from, to, shouldNormalize: false);
//To get 0-2pi (360 degrees) we can use the determinant [a, b, u] = (a x b) dot u
//Where u is a reference up vector
//Remember that the cross product is not alwayspointing up - it can change to down depending on how the vectors are aligned
//Which is why we need a fixed reference up
MyVector3 cross = MyVector3.Cross(from, to);
float determinant = MyVector3.Dot(MyVector3.Cross(from, to), upRef);
//Debug.Log(determinant);
if (determinant >= 0f)
{
return(angleRad);
}
else
{
return((Mathf.PI * 2f) - angleRad);
}
}
//Get the forward direction do the Bezier Quadratic
//This direction is always tangent to the curve
public static MyVector3 BezierQuadraticForwardDir(MyVector3 posA, MyVector3 posB, MyVector3 handlePos, float t)
{
//Same as when we calculate t
MyVector3 interpolation_posA_handlePos = BezierLinear(posA, handlePos, t);
MyVector3 interpolation_handlePos_posB = BezierLinear(handlePos, posB, t);
MyVector3 forwardDir = MyVector3.Normalize(interpolation_handlePos_posB - interpolation_posA_handlePos);
return(forwardDir);
}
//3d
public static MyVector3 GetLinePlaneIntersectionPoint(Plane3 plane, Edge3 line)
{
MyVector3 lineDir = MyVector3.Normalize(line.p1 - line.p2);
Ray3 ray = new Ray3(line.p1, lineDir);
MyVector3 intersectionPoint = GetIntersectionCoordinate(plane, ray);
return(intersectionPoint);
}
//If we have a forward and an up reference vector
//So this is not going to work if we have loops
//tangent is same as forward
public InterpolationTransform(MyVector3 position, MyVector3 tangent, MyVector3 up)
{
this.position = position;
MyVector3 biNormal = MyVector3.Normalize(MyVector3.Cross(up, tangent));
MyVector3 normal = MyVector3.Normalize(MyVector3.Cross(tangent, biNormal));
this.orientation = Quaternion.LookRotation(tangent.ToVector3(), normal.ToVector3());
}
//
// Calculate the normal of a clock-wise oriented triangle in 3d space
//
public static MyVector3 CalculateTriangleNormal(MyVector3 p1, MyVector3 p2, MyVector3 p3, bool shouldNormalize = true)
{
MyVector3 normal = MyVector3.Cross(p3 - p2, p1 - p2);
if (shouldNormalize)
{
normal = MyVector3.Normalize(normal);
}
return(normal);
}
//
// Get transforms (position and orientation) at point t
//
//The position and the tangent are easy to find
//what's difficult to find is the normal because a line doesn't have a single normal
//To get the normal in 2d, we can just flip two coordinates in the forward vector and set one to negative
//MyVector3 normal = new MyVector3(-forwardDir.z, 0f, forwardDir.x);
//In 3d there are multiple alternatives:
//You can read about these methods here:
//https://pomax.github.io/bezierinfo/#pointvectors3d
//Game Programming Gems 2: The Parallel Transport Frame (p. 215)
//Unite 2015 - A coder's guide to spline-based procedural geometry https://www.youtube.com/watch?v=o9RK6O2kOKo
//
// Alternative 1. Fixed up
//
//Use ref vector to know which direction is up
//Is not going to work if we have loops, but should work if you make "2d" roads like in cities skylines so no roller coasters
public static MyQuaternion GetOrientation_UpRef(MyVector3 tangent, MyVector3 upRef)
{
tangent = MyVector3.Normalize(tangent);
MyVector3 biNormal = MyVector3.Normalize(MyVector3.Cross(upRef, tangent));
MyVector3 normal = MyVector3.Normalize(MyVector3.Cross(tangent, biNormal));
MyQuaternion orientation = new MyQuaternion(tangent, normal);
return(orientation);
}
//
// Add a triangle to this mesh
//
//We dont have a normal so we have to calculate it, so make sure v1-v2-v3 is clock-wise
public HalfEdgeFace3 AddTriangle(MyVector3 p1, MyVector3 p2, MyVector3 p3, bool findOppositeEdge = false)
{
MyVector3 normal = MyVector3.Normalize(MyVector3.Cross(p3 - p2, p1 - p2));
MyMeshVertex v1 = new MyMeshVertex(p1, normal);
MyMeshVertex v2 = new MyMeshVertex(p2, normal);
MyMeshVertex v3 = new MyMeshVertex(p3, normal);
HalfEdgeFace3 f = AddTriangle(v1, v2, v3);
return(f);
}
//
// Tangent at point t (Forward direction if we travel along the curve)
//
public static MyVector3 GetTangent(MyVector3 posA, MyVector3 posB, MyVector3 handlePos, float t)
{
t = Mathf.Clamp01(t);
//Alternative 1
//Same as when we calculate position from t
//MyVector3 interpolation_posA_handlePos = BezierLinear.GetPosition(posA, handlePos, t);
//MyVector3 interpolation_handlePos_posB = BezierLinear.GetPosition(handlePos, posB, t);
//MyVector3 tangent = MyVector3.Normalize(interpolation_handlePos_posB - interpolation_posA_handlePos);
//Alternative 2
//The tangent is also the derivative vector
MyVector3 tangent = MyVector3.Normalize(GetDerivativeVec(posA, posB, handlePos, t));
return(tangent);
}
//
// Alternative 2. Frenet normal (also known as Frenet Frame)
//
//Use the tagent we have and a tangent next to it
//Works in many cases (but sometimes the frame may flip because of changes in the second derivative)
public static MyQuaternion GetOrientation_FrenetNormal(MyVector3 tangent, MyVector3 secondDerivativeVec)
{
MyVector3 a = MyVector3.Normalize(tangent);
//What a next point's tangent would be if the curve stopped changing at our point and just had the same derivative and second derivative from that point on
MyVector3 b = MyVector3.Normalize(a + secondDerivativeVec);
//A vector that we use as the "axis of rotation" for turning the tangent a quarter circle to get the normal
MyVector3 r = MyVector3.Normalize(MyVector3.Cross(a, b));
//The normal vector should be perpendicular to the plane that the tangent and the axis of rotation lie in
MyVector3 normal = MyVector3.Normalize(MyVector3.Cross(r, a));
MyQuaternion orientation = new MyQuaternion(tangent, normal);
return(orientation);
}
//
// Alternative 1.5. Similar to Alternative 1, but we know the up vector at both the start and end position
//
public static InterpolationTransform GetTransform_InterpolateBetweenUpVectors(
_Curve curve, float t, MyVector3 upRefStart, MyVector3 upRefEnd)
{
//Position on the curve at point t
MyVector3 pos = curve.GetPosition(t);
//Forward direction (tangent) on the curve at point t
MyVector3 forwardDir = curve.GetTangent(t);
//Interpolate between the start and end up vector to get an up vector at a t position
MyVector3 interpolatedUpDir = MyVector3.Normalize(BezierLinear.GetPosition(upRefStart, upRefEnd, t));
MyQuaternion orientation = InterpolationTransform.GetOrientation_UpRef(forwardDir, interpolatedUpDir);
InterpolationTransform trans = new InterpolationTransform(pos, orientation);
return(trans);
}
//
// Get a Transform (includes position and orientation) at point t
//
public InterpolationTransform GetTransform(float t)
{
//Same as when we calculate t
MyVector3 interpolation_1_2 = _Interpolation.BezierQuadratic(posA, handleB, handleA, t);
MyVector3 interpolation_2_3 = _Interpolation.BezierQuadratic(posA, posB, handleB, t);
MyVector3 finalInterpolation = _Interpolation.BezierLinear(interpolation_1_2, interpolation_2_3, t);
//This direction is always tangent to the curve
MyVector3 forwardDir = MyVector3.Normalize(interpolation_2_3 - interpolation_1_2);
//A simple way to get the other directions is to use LookRotation with just forward dir as parameter
//Then the up direction will always be the world up direction, and it calculates the right direction
Quaternion orientation = Quaternion.LookRotation(forwardDir.ToVector3());
InterpolationTransform trans = new InterpolationTransform(finalInterpolation, orientation);
return(trans);
}
//The angle between two vectors 0 <= angle <= 180
//Same as Vector3.Angle() but we are using MyVector3
public static float AngleBetween(MyVector3 from, MyVector3 to, bool shouldNormalize = true)
{
//from and to should be normalized
//But sometimes they are already normalized and then we dont need to do it again
if (shouldNormalize)
{
from = MyVector3.Normalize(from);
to = MyVector3.Normalize(to);
}
//dot(a_normalized, b_normalized) = cos(alpha) -> acos(dot(a_normalized, b_normalized)) = alpha
float dot = MyVector3.Dot(from, to);
//This shouldn't happen but may happen because of floating point precision issues
dot = Mathf.Clamp(dot, -1f, 1f);
float angleRad = Mathf.Acos(dot);
return(angleRad);
}
//Cut a triangle where two vertices are inside and the other vertex is outside
//Make sure they are sorted clockwise: O1-O2-I1
//F means that this vertex is outside the plane
private static void CutTriangleTwoOutside(MyMeshVertex O1, MyMeshVertex O2, MyMeshVertex I1, HalfEdgeData3 newMeshO, HalfEdgeData3 newMeshI, HashSet <HalfEdge3> newEdgesI, HashSet <HalfEdge3> newEdgesO, Plane3 cutPlane)
{
//Cut the triangle by using edge-plane intersection
//Triangles in Unity are ordered clockwise, so form edges that intersects with the plane:
Edge3 e_O2I1 = new Edge3(O2.position, I1.position);
//Edge3 e_F1F2 = new Edge3(F1, F2); //Not needed because never intersects with the plane
Edge3 e_I1O1 = new Edge3(I1.position, O1.position);
//The positions of the intersection vertices
MyVector3 pos_O2I1 = _Intersections.GetLinePlaneIntersectionPoint(cutPlane, e_O2I1);
MyVector3 pos_I1O1 = _Intersections.GetLinePlaneIntersectionPoint(cutPlane, e_I1O1);
//The normals of the intersection vertices
float percentageBetween_O2I1 = MyVector3.Distance(O2.position, pos_O2I1) / MyVector3.Distance(O2.position, I1.position);
float percentageBetween_I1O1 = MyVector3.Distance(I1.position, pos_I1O1) / MyVector3.Distance(I1.position, O1.position);
MyVector3 normal_O2I1 = _Interpolation.Lerp(O2.normal, I1.normal, percentageBetween_O2I1);
MyVector3 normal_I1O1 = _Interpolation.Lerp(I1.normal, O1.normal, percentageBetween_I1O1);
//MyVector3 normal_F2B1 = Vector3.Slerp(F2.normal.ToVector3(), B1.normal.ToVector3(), percentageBetween_F2B1).ToMyVector3();
//MyVector3 normal_B1F1 = Vector3.Slerp(B1.normal.ToVector3(), F1.normal.ToVector3(), percentageBetween_B1F1).ToMyVector3();
normal_O2I1 = MyVector3.Normalize(normal_O2I1);
normal_I1O1 = MyVector3.Normalize(normal_I1O1);
//The intersection vertices
MyMeshVertex v_O2I1 = new MyMeshVertex(pos_O2I1, normal_O2I1);
MyMeshVertex v_I1O1 = new MyMeshVertex(pos_I1O1, normal_I1O1);
//Form 3 new triangles
//Outside
AddTriangleToMesh(v_O2I1, v_I1O1, O2, newMeshO, newEdgesO);
AddTriangleToMesh(O2, v_I1O1, O1, newMeshO, null);
//Inside
AddTriangleToMesh(v_I1O1, v_O2I1, I1, newMeshI, newEdgesI);
}
//Remove flat tetrahedrons (a vertex in a triangle)
private static bool RemoveFlatTetrahedrons(HalfEdgeData3 meshData, Normalizer3 normalizer = null)
{
HashSet <HalfEdgeVertex3> vertices = meshData.verts;
bool foundFlatTetrahedron = false;
foreach (HalfEdgeVertex3 vertex in vertices)
{
HashSet <HalfEdge3> edgesGoingToVertex = vertex.GetEdgesPointingToVertex(meshData);
if (edgesGoingToVertex.Count == 3)
{
//Find the vertices of the triangle covering this vertex clock-wise
HalfEdgeVertex3 v1 = vertex.edge.v;
HalfEdgeVertex3 v2 = vertex.edge.prevEdge.oppositeEdge.v;
HalfEdgeVertex3 v3 = vertex.edge.oppositeEdge.nextEdge.v;
//Build a plane
MyVector3 normal = MyVector3.Normalize(MyVector3.Cross(v3.position - v2.position, v1.position - v2.position));
Plane3 plane = new Plane3(v1.position, normal);
//Find the distance from the vertex to the plane
float distance = _Geometry.GetSignedDistanceFromPointToPlane(vertex.position, plane);
distance = Mathf.Abs(distance);
if (distance < FLAT_TETRAHEDRON_DISTANCE)
{
//Debug.Log("Found flat tetrahedron");
Vector3 p1 = normalizer.UnNormalize(v1.position).ToVector3();
Vector3 p2 = normalizer.UnNormalize(v2.position).ToVector3();
Vector3 p3 = normalizer.UnNormalize(v3.position).ToVector3();
TestAlgorithmsHelpMethods.DebugDrawTriangle(p1, p2, p3, normal.ToVector3(), Color.blue, Color.red);
foundFlatTetrahedron = true;
//Save the opposite edges
HashSet <HalfEdge3> oppositeEdges = new HashSet <HalfEdge3>();
oppositeEdges.Add(v1.edge.oppositeEdge);
oppositeEdges.Add(v2.edge.oppositeEdge);
oppositeEdges.Add(v3.edge.oppositeEdge);
//Remove the three triangles
foreach (HalfEdge3 e in edgesGoingToVertex)
{
meshData.DeleteFace(e.face);
}
//Add the new triangle (could maybe connect it ourselves)
HalfEdgeFace3 newTriangle = meshData.AddTriangle(v1.position, v2.position, v3.position, findOppositeEdge: false);
meshData.TryFindOppositeEdge(newTriangle.edge, oppositeEdges);
meshData.TryFindOppositeEdge(newTriangle.edge.nextEdge, oppositeEdges);
meshData.TryFindOppositeEdge(newTriangle.edge.nextEdge.nextEdge, oppositeEdges);
break;
}
}
}
return(foundFlatTetrahedron);
}