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);
}
}
void Update()
{
baseTransformMatrix = new DualMatrix4x3(baseTransform.position, baseTransform.rotation, baseTransform.localScale);
//Set up the variables to optimize
optimizer.Add(transform.position, "position");
optimizer.Add(transform.up, "yBasis");
optimizer.Add(transform.forward, "zBasis");
optimizer.Add(transform.localScale, "scale");
optimizer.CalculateErrorDerivative = CalculateErrorDerivative;
//Calculate and Apply the Instantaneous Gradients at this point in time
transform.position -= optimizer.CalcErrorGradient("position") * alpha;
Vector3 forwardBasis = transform.forward - (optimizer.CalcErrorGradient("zBasis") * alpha);
Vector3 upBasis = transform.up - (optimizer.CalcErrorGradient("yBasis") * alpha);
transform.rotation = Quaternion.LookRotation(forwardBasis, upBasis);
transform.localScale -= optimizer.CalcErrorGradient("scale") * alpha;
}