/// <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); }