public override Cost ComputeCost(Transform t) { DistanceCost c = new DistanceCost() { ToTarget = t.position, TargetPosition = t.position, ToTargetRotation = t.rotation.eulerAngles, TargetRotation = new float3x3(t.right, t.up, t.forward) }; return(c); }
public static DistanceCost ComputeCost(Transform from, Transform to, Transform root) { float3x3 fRot = new float3x3(root.InverseTransformDirection(from.right), root.InverseTransformDirection(from.up), root.InverseTransformDirection(from.forward)); float3x3 tRot = new float3x3(root.InverseTransformDirection(to.right), root.InverseTransformDirection(to.up), root.InverseTransformDirection(to.forward)); DistanceCost c = new DistanceCost() { ToTarget = root.InverseTransformPoint(to.position - from.position), ToTargetRotation = new float3( math.acos(math.dot(tRot.c0, fRot.c0)), math.acos(math.dot(tRot.c1, fRot.c1)), math.acos(math.dot(tRot.c2, fRot.c2))), TargetPosition = root.InverseTransformPoint(to.transform.position), TargetRotation = tRot }; return(c); }