void constrainVertsToDeformation(Vector3[] originalVerts, Matrix4x4 rigidOrigToBody, ref NativeArray <Vector3> bodyVerts)
{
Matrix4x4 bodyToOrig = rigidOrigToBody.inverse;
for (int i = 0; i < bodyVerts.Length; i++)
{
bodyVerts[i] = Vector3.Lerp(bodyToOrig.MultiplyPoint3x4(bodyVerts[i]), originalVerts[i], stiffness);
}
//Do a bunch of fitting iterations
float alpha = 0.45f;
for (int i = 0; i < 5; i++)
{
optimizer.Add(xScale, "xScale");
optimizer.Add(yBasis, "yBasis");
optimizer.Add(zBasis, "zBasis");
xScale -= optimizer.CalcErrorDerivative("xScale") * alpha;
yBasis -= optimizer.CalcErrorGradient("yBasis") * alpha;
zBasis -= optimizer.CalcErrorGradient("zBasis") * alpha;
//Prevent shape from inverting
if (xScale < 0f)
{
xScale *= -1f; yBasis *= -1f;
}
}
//xScale = Mathf.Lerp(xScale, 1f, stiffness);
//yBasis = Vector3.Lerp(yBasis, yBasis.normalized, stiffness);
//zBasis = Vector3.Lerp(zBasis, zBasis.normalized, stiffness);
//Do Volume Preservation Stuff
Vector3 xBasis = Vector3.Cross(yBasis, zBasis).normalized *xScale;
for (int i = 0; i < bodyVerts.Length; i++)
{
DualVector3 deformedOriginal = (xBasis.normalized * Vector3.Dot(xBasis, originalVerts[i])) +
(yBasis.normalized * Vector3.Dot(yBasis, originalVerts[i])) +
(zBasis.normalized * Vector3.Dot(zBasis, originalVerts[i]));
bodyVerts[i] = rigidOrigToBody.MultiplyPoint3x4(deformedOriginal);
}
}
