/// <summary>
/// Constructs a constraint required no other surfaces be included in this shape
/// </summary>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_NoOtherSurface()
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.NoOtherSurface;
return(constraint);
}
/// <summary>
/// Constructs a constraint requiring the shape to be away from all walls
/// </summary>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_AwayFromWalls()
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.AwayFromWalls;
return(constraint);
}
/// <summary>
/// Constructs a constraint requiring component B to be immediately to the right of component A.
/// </summary>
/// <param name="componentIndexA"></param>
/// <param name="componentIndexB"></param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_AtRightOf(int componentIndexA, int componentIndexB)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.AtRightOf;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
return(constraint);
}
/// <summary>
/// Constructs a constraint requiring the components shapes longer edges
/// to have perpendicular alignment.
/// </summary>
/// <param name="componentIndexA">Zero based index of the first component constraint</param>
/// <param name="componentIndexB">Zero based index of the second component constraint</param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_RectanglesPerpendicular(int componentIndexA, int componentIndexB)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.RectanglesPerpendicular;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
return(constraint);
}
// TODO: this mutates restPositions...unsure what to do about this
static ShapeConstraint CreateShapeConstraint(NativeArray <float3> restPositions, NativeArray <float3> positions, int Start, int End, float Stiffness)
{
float a00 = 0f;
float a01 = 0f;
float a02 = 0f;
float a10 = 0f;
float a11 = 0f;
float a12 = 0f;
float a20 = 0f;
float a21 = 0f;
float a22 = 0f;
float inverseMass = 1; // Could be defined on the atoms themselves
float totalMass = 0f;
ShapeConstraint sc = new ShapeConstraint();
sc.end = End;
sc.start = Start;
sc.stiffness = Stiffness;
sc.restCenterOfMass = Vector3.zero;
// compute center of mass of resting pose
for (var i = sc.start; i < sc.end; i++)
{
sc.restCenterOfMass += positions[i] * inverseMass;
totalMass += inverseMass;
}
sc.restCenterOfMass /= totalMass;
// compute rest matrix
for (var i = sc.start; i < sc.end; i++)
{
float3 q = positions[i] - sc.restCenterOfMass;
a00 += inverseMass * q.x * q.x;
a01 += inverseMass * q.x * q.y;
a02 += inverseMass * q.x * q.z;
a10 += inverseMass * q.y * q.x;
a11 += inverseMass * q.y * q.y;
a12 += inverseMass * q.y * q.z;
a20 += inverseMass * q.z * q.x;
a21 += inverseMass * q.z * q.y;
a22 += inverseMass * q.z * q.z;
restPositions[i] = q;
}
float3x3 restMatrix = new float3x3(
a00, a01, a02,
a10, a11, a12,
a20, a21, a22
);
sc.inverseMassMatrix = math.inverse(restMatrix);
return(sc);
}
/// <summary>
/// Constructs a constraint requiring the components shapes longest edges to
/// have the same length, within the difference parameter.
///
/// The difference is defined as the ratio of the longest edges of the two components.
/// </summary>
/// <param name="componentIndexA">Zero based index of the first component constraint</param>
/// <param name="componentIndexB">Zero based index of the second component constraint</param>
/// <param name="similarityMin">Maximum similarity</param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_RectanglesSameLength(int componentIndexA, int componentIndexB, float similarityMin = 0.8f)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.RectanglesSameLength;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
constraint.Param_Float_0 = similarityMin;
return(constraint);
}
/// <summary>
/// Constructs a constraint requiring the components shapes to be either aligned
/// with parallel or parallel alignment. The difference is the defined as the cosine of the angle
/// between the best aligned axis (i.e. the dot product)
/// </summary>
/// <param name="componentIndexA">Zero based index of the first component constraint</param>
/// <param name="componentIndexB">Zero based index of the second component constraint</param>
/// <param name="maxDifference">Maximum difference</param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_RectanglesAligned(int componentIndexA, int componentIndexB, float maxDifference = 0.1f)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.RectanglesAligned;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
constraint.Param_Float_0 = maxDifference;
return(constraint);
}
/// <summary>
/// Constructs a constraint requiring component B to be immediately to the left of component A.
/// </summary>
/// <param name="componentIndexA"></param>
/// <param name="componentIndexB"></param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_AtLeftOf(int componentIndexA, int componentIndexB)
{
ShapeConstraint constraint = new ShapeConstraint
{
Type = ShapeConstraintType.AtLeftOf,
Param_Int_0 = componentIndexA,
Param_Int_1 = componentIndexB
};
return(constraint);
}
/// <summary>
/// Constructs a constraint requiring the components shapes longer edges
/// to have parallel alignment.
/// </summary>
/// <param name="componentIndexA">Zero based index of the first component constraint</param>
/// <param name="componentIndexB">Zero based index of the second component constraint</param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_RectanglesParallel(int componentIndexA, int componentIndexB)
{
ShapeConstraint constraint = new ShapeConstraint
{
Type = ShapeConstraintType.RectanglesParallel,
Param_Int_0 = componentIndexA,
Param_Int_1 = componentIndexB
};
return(constraint);
}
/// <summary>
/// Constructs a constraint requiring the components shapes to be either aligned
/// with parallel or parallel alignment. The difference is the defined as the cosine of the angle
/// between the best aligned axis (i.e. the dot product)
/// </summary>
/// <param name="componentIndexA">Zero based index of the first component constraint</param>
/// <param name="componentIndexB">Zero based index of the second component constraint</param>
/// <param name="maxDifference">Maximum difference</param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_RectanglesAligned(int componentIndexA, int componentIndexB, float maxDifference = 0.1f)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.RectanglesAligned;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
constraint.Param_Float_0 = maxDifference;
return constraint;
}
/// <summary>
/// Constructs a constraint requiring the components shapes longer edges
/// to have perpendicular alignment.
/// </summary>
/// <param name="componentIndexA">Zero based index of the first component constraint</param>
/// <param name="componentIndexB">Zero based index of the second component constraint</param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_RectanglesPerpendicular(int componentIndexA, int componentIndexB)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.RectanglesPerpendicular;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
return constraint;
}
/// <summary>
/// Constructs a constraint requiring the shape to be away from all walls
/// </summary>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_AwayFromWalls()
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.AwayFromWalls;
return constraint;
}
/// <summary>
/// Constructs a constraint required no other surfaces be included in this shape
/// </summary>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_NoOtherSurface()
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.NoOtherSurface;
return constraint;
}
public void Execute()
{
float invIterations = 1f / (float)iterationCount;
float inverseMass = 1f; // THIS SHOULD VARY BY ATOM! add native array of inversemasses to this overall solver
for (var k = 0; k < iterationCount; k++)
{
for (var j = 0; j < distanceConstraints.Length; j++)
{
// TODO: could use local refs here to avoid stupid copying
var dc = distanceConstraints[j];
var a = predicteds[dc.i0];
var b = predicteds[dc.i1];
var delta = a - b;
var direction = math.normalize(delta);
var C = math.length(delta) - dc.distance;
var dstiffness = 1f - math.pow(1f - dc.stiffness, invIterations);
if (C > -float.Epsilon && C < float.Epsilon)
{
continue;
}
predicteds[dc.i0] -= inverseMass / (inverseMass + inverseMass) * dstiffness * C * direction;
predicteds[dc.i1] += inverseMass / (inverseMass + inverseMass) * dstiffness * C * direction;
}
for (var j = 0; j < shapeConstraints.Length; j++)
{
ShapeConstraint sc = shapeConstraints[j];
float dstiffness = 1f - Mathf.Pow(1f - sc.stiffness, invIterations);
float totalMass = 0f;
float3 centerOfMass = Vector3.zero;
for (var i = sc.start; i < sc.end; i++)
{
centerOfMass += predicteds[i] * inverseMass;
totalMass += inverseMass;
}
centerOfMass /= totalMass;
// compute rest matrix
float a00 = 0f;
float a01 = 0f;
float a02 = 0f;
float a10 = 0f;
float a11 = 0f;
float a12 = 0f;
float a20 = 0f;
float a21 = 0f;
float a22 = 0f;
for (var i = sc.start; i < sc.end; i++)
{
float3 q = restPositions[i];
float3 p = predicteds[i] - centerOfMass;
a00 += inverseMass * p.x * q.x;
a01 += inverseMass * p.x * q.y;
a02 += inverseMass * p.x * q.z;
a10 += inverseMass * p.y * q.x;
a11 += inverseMass * p.y * q.y;
a12 += inverseMass * p.y * q.z;
a20 += inverseMass * p.z * q.x;
a21 += inverseMass * p.z * q.y;
a22 += inverseMass * p.z * q.z;
}
float3x3 currentMatrix = new float3x3(
a00, a01, a02,
a10, a11, a12,
a20, a21, a22
);
float3x3 covarianceMatrix = math.mul(currentMatrix, sc.inverseMassMatrix);
float3x3 rotationMatrix = Decomposition.FastExtractRotationFrom(covarianceMatrix, 10);
for (var i = sc.start; i < sc.end; i++)
{
float3 goal = centerOfMass + math.mul(rotationMatrix, restPositions[i]);
predicteds[i] += (goal - predicteds[i]) * sc.stiffness * dstiffness;
}
}
for (var i = 0; i < predicteds.Length; i++)
{
if (predicteds[i].y > 0)
{
continue;
}
var aboveGround = predicteds[i];
aboveGround.y = 0;
predicteds[i] = aboveGround;
}
for (var j = 0; j < positionConstraints.Length; j++)
{
// TODO: could use local refs here to avoid stupid copying
var pc = positionConstraints[j];
predicteds[pc.i] = pc.position;
}
}
}
/// <summary>
/// Constructs a constraint requiring the components shapes longest edges to
/// have the same length, within the difference difference parameter.
///
/// The difference is defined as the ratio of the longest edges of the two components.
/// </summary>
/// <param name="componentIndexA">Zero based index of the first component constraint</param>
/// <param name="componentIndexB">Zero based index of the second component constraint</param>
/// <param name="similarityMin">Maximum similarity</param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_RectanglesSameLength(int componentIndexA, int componentIndexB, float similarityMin = 0.8f)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.RectanglesSameLength;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
constraint.Param_Float_0 = similarityMin;
return constraint;
}
/// <summary>
/// Constructs a constraint requiring component B to be immediately to the right of component A.
/// </summary>
/// <param name="componentIndexA"></param>
/// <param name="componentIndexB"></param>
/// <returns>Constructed shape constraint</returns>
public static ShapeConstraint Create_AtRightOf(int componentIndexA, int componentIndexB)
{
ShapeConstraint constraint = new ShapeConstraint();
constraint.Type = ShapeConstraintType.AtRightOf;
constraint.Param_Int_0 = componentIndexA;
constraint.Param_Int_1 = componentIndexB;
return constraint;
}
void Spawn()
{
if (mesh == null)
{
return;
}
if (solver == null)
{
return;
}
if (AtomPrefab == null)
{
return;
}
var vertices = mesh.vertices;
var triangles = mesh.triangles;
var positionToAtomMap = new Dictionary <Vector3, Atom>();
foreach (var a in solver.atoms)
{
Destroy(a.gameObject);
}
solver.atoms.Clear();
solver.positionConstraints.Clear();
solver.distanceConstraints.Clear();
solver.shapeConstraints.Clear();
// create all atoms that occupy unique positions
for (var i = 0; i < vertices.Length; i++)
{
var modelSpacePosition = vertices[i];
if (positionToAtomMap.ContainsKey(modelSpacePosition))
{
continue;
}
var worldSpacePosition = transform.TransformPoint(modelSpacePosition);
var atom = Instantiate(AtomPrefab, transform);
atom.transform.position = worldSpacePosition;
atom.velocity = Vector3.zero;
atom.predicted = Vector3.zero;
atom.name = i.ToString();
atom.diameter = DIAMETER;
atom.inverseMass = 1f;
solver.atoms.Add(atom);
positionToAtomMap.Add(modelSpacePosition, atom);
}
for (var i = 0; i < triangles.Length; i += 3)
{
var i1 = triangles[i + 0];
var i2 = triangles[i + 1];
var i3 = triangles[i + 2];
var p1 = vertices[i1];
var p2 = vertices[i2];
var p3 = vertices[i3];
var a1 = positionToAtomMap[p1];
var a2 = positionToAtomMap[p2];
var a3 = positionToAtomMap[p3];
var d1 = Vector3.Distance(a1.transform.position, a2.transform.position);
var d2 = Vector3.Distance(a1.transform.position, a3.transform.position);
var d3 = Vector3.Distance(a2.transform.position, a3.transform.position);
solver.distanceConstraints.Add(new DistanceConstraint()
{
a = a1, b = a2, distance = d1, stiffness = STIFFNESS
});
solver.distanceConstraints.Add(new DistanceConstraint()
{
a = a1, b = a3, distance = d2, stiffness = STIFFNESS
});
solver.distanceConstraints.Add(new DistanceConstraint()
{
a = a2, b = a3, distance = d3, stiffness = STIFFNESS
});
}
var sc = new ShapeConstraint(solver.atoms, STIFFNESS);
solver.shapeConstraints.Add(sc);
solver.atoms[0].inverseMass = 1;
var pc = new PositionConstraint
{
position = PIN_POINT,
a = solver.atoms[0]
};
solver.positionConstraints.Add(pc);
}
