/// <summary>
/// Calculates value and derivative of the position of the sphere with respect to whichDeriv
/// </summary>
float CalculateErrorDerivative()
{
DualVector3 thisYBasis = optimizer.vec("yBasis"), thisZBasis = optimizer.vec("zBasis");
thisYBasis = thisYBasis.Normalize(); thisZBasis = thisZBasis.Normalize();
DualVector3 thisXBasis = thisYBasis.Cross(thisZBasis);
thisYBasis = thisZBasis.Cross(thisXBasis);
//THIS IS A FULLY DIFFERENTIABLE RIGID TRANSFORM - NO SINGULARITIES
//Also Single Covers the space (when non-uniform scaling is disabled)
DualVector3 scale = optimizer.vec("scale");
DualMatrix4x3 thisTransform = new DualMatrix4x3(thisXBasis * scale.x, thisYBasis * scale.y, thisZBasis * scale.z, optimizer.vec("position"));
//Sum the Squared Errors
DualNumber error = new DualNumber();
for (int i = 0; i < icoVerts.Length; i++)
{
DualVector3 thisPoint = thisTransform * icoVerts[i]; DualVector3 basePoint = baseTransformMatrix * icoVerts[i];
if (drawDebug)
{
Debug.DrawLine(thisPoint, basePoint);
}
error += (thisPoint - basePoint).SqrMagnitude();
}
//Divide by Number of Squared Errors
error /= icoVerts.Length;
return(error.Derivative);
}
/// <summary>
/// Calculates squared distance of a point from the surface of a sphere
/// </summary>
DualNumber DistFromSurface(Vector3 spherePosition, float radius, DualVector3 point, bool draw)
{
if (draw)
{
Debug.DrawLine(point, spherePosition + ((point.Value - spherePosition).normalized * radius));
}
return(((DualVector3)spherePosition - point).Magnitude() - radius);
}
/// <summary>
/// Calculates value and derivative of the position of the arm with respect to whichDeriv
/// </summary>
DualNumber ArmFKError(Vector3 clampedPos, int whichDeriv)
{
DualNumber thetaVar = whichDeriv == 0 ? DualNumber.Variable(theta) : DualNumber.Constant(theta);
DualNumber phiVar = whichDeriv == 1 ? DualNumber.Variable(phi) : DualNumber.Constant(phi);
baseJoint = new DualVector3(Math.Sin(thetaVar), Math.Cos(thetaVar), new DualNumber());
endJoint = baseJoint + new DualVector3(Math.Sin(thetaVar + phiVar), Math.Cos(thetaVar + phiVar), new DualNumber());
return(((DualVector3)clampedPos - endJoint).SqrMagnitude());
}
/// <summary>
/// Calculates value and derivative of the position of the sphere with respect to whichDeriv
/// </summary>
DualNumber SphereFittingError(int whichDeriv, bool drawDebug)
{
DualVector3 thisPosition = new DualVector3(
whichDeriv == 0 ? DualNumber.Variable(transform.position.x) : DualNumber.Constant(transform.position.x),
whichDeriv == 1 ? DualNumber.Variable(transform.position.y) : DualNumber.Constant(transform.position.y),
whichDeriv == 2 ? DualNumber.Variable(transform.position.z) : DualNumber.Constant(transform.position.z));
DualVector3 thisYBasis = new DualVector3(
whichDeriv == 3 ? DualNumber.Variable(transform.up.x) : DualNumber.Constant(transform.up.x),
whichDeriv == 4 ? DualNumber.Variable(transform.up.y) : DualNumber.Constant(transform.up.y),
whichDeriv == 5 ? DualNumber.Variable(transform.up.z) : DualNumber.Constant(transform.up.z));
DualVector3 thisZBasis = new DualVector3(
whichDeriv == 6 ? DualNumber.Variable(transform.forward.x) : DualNumber.Constant(transform.forward.x),
whichDeriv == 7 ? DualNumber.Variable(transform.forward.y) : DualNumber.Constant(transform.forward.y),
whichDeriv == 8 ? DualNumber.Variable(transform.forward.z) : DualNumber.Constant(transform.forward.z));
DualVector3 thisScale = new DualVector3(
whichDeriv == 9 ? DualNumber.Variable(transform.localScale.x) : DualNumber.Constant(transform.localScale.x),
whichDeriv == 10 ? DualNumber.Variable(transform.localScale.y) : DualNumber.Constant(transform.localScale.y),
whichDeriv == 11 ? DualNumber.Variable(transform.localScale.z) : DualNumber.Constant(transform.localScale.z));
thisYBasis = thisYBasis.Normalize(); thisZBasis = thisZBasis.Normalize();
DualVector3 thisXBasis = thisYBasis.Cross(thisZBasis);
thisYBasis = thisZBasis.Cross(thisXBasis);
//THIS IS A FULLY DIFFERENTIABLE RIGID TRANSFORM - NO SINGULARITIES
DualMatrix4x3 thisTransform = new DualMatrix4x3(thisXBasis * thisScale.x, thisYBasis * thisScale.y, thisZBasis * thisScale.z, thisPosition);
//Sum the Squared Errors
DualNumber error = new DualNumber();
for (int i = 0; i < icoVerts.Length; i++)
{
DualVector3 thisPoint = thisTransform * icoVerts[i]; DualVector3 basePoint = baseTransformMatrix * icoVerts[i];
if (drawDebug)
{
Debug.DrawLine(thisPoint, basePoint);
}
error += (thisPoint - basePoint).SqrMagnitude();
}
//Divide by Number of Squared Errors
error /= icoVerts.Length;
return(error);
}
public Vector3 CalcErrorGradient(string vectorKey)
{
DualVector3 vector = new DualVector3();
if (CalculateErrorDerivative != null && vectors.TryGetValue(vectorKey, out vector))
{
Vector3 errorGradient = Vector3.zero;
for (int i = 0; i < 3; i++)
{
vectors[vectorKey] = new DualVector3(vector.Value, i);
errorGradient[i] = CalculateErrorDerivative();
}
vectors[vectorKey] = new DualVector3(vector.Value);
return(errorGradient);
}
return(Vector3.zero);
}
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);
}
}
/// <summary>
/// Calculates value and derivative of the position of the sphere with respect to whichDeriv
/// </summary>
DualNumber SphereFittingError(int whichDeriv, bool drawDebug)
{
DualVector3 spherePosition = new DualVector3(
whichDeriv == 0 ? DualNumber.Variable(transform.position.x) : DualNumber.Constant(transform.position.x),
whichDeriv == 1 ? DualNumber.Variable(transform.position.y) : DualNumber.Constant(transform.position.y),
whichDeriv == 2 ? DualNumber.Variable(transform.position.z) : DualNumber.Constant(transform.position.z));
DualNumber radius = whichDeriv == 3 ? DualNumber.Variable(transform.localScale.x) : DualNumber.Constant(transform.localScale.x);
//Sum the Squared Errors
DualNumber error = new DualNumber();
for (int i = 0; i < icoVerts.Length; i++)
{
error += DistFromSurface(baseSphere.position, baseSphere.localScale.x * 0.5f, spherePosition + (radius * icoVerts[i]), drawDebug).Squared();
}
//Divide by Number of Squared Errors
error /= icoVerts.Length;
return(error);
}
/// <summary>
/// Calculates value and derivative of the position of the sphere with respect to whichDeriv
/// </summary>
float CalculateErrorDerivative()
{
DualVector3 thisYBasis = optimizer.vec("yBasis"), thisZBasis = optimizer.vec("zBasis"),
thisXBasis = optimizer.num("xScale") * (thisYBasis.Cross(thisZBasis)).Normalize();
//thisYBasis = /*thisYBasis.Magnitude() */ (thisZBasis.Cross(thisXBasis)).Normalize();
//Sum the Squared Errors
DualNumber error = new DualNumber();
for (int i = 0; i < originalVerts.Length; i++)
{
DualVector3 thisPoint = (thisXBasis * thisXBasis.Normalize().Dot(originalVerts[i])) +
(thisYBasis * thisYBasis.Normalize().Dot(originalVerts[i])) +
(thisZBasis * thisZBasis.Normalize().Dot(originalVerts[i]));
DualVector3 basePoint = bodyVerts[i];
//if (true) { Debug.DrawLine(thisPoint, basePoint); }
error += (thisPoint - basePoint).SqrMagnitude();
}
//Divide by Number of Squared Errors
error /= originalVerts.Length;
return(error.Derivative);
}
