Pulling my hair out trying to figure out this math problem

I'm a brain-dead programmer by trade, so I was hoping to enlist the help of someone more math inclined.

I've created a self-correcting pivot. Essentially a pin that attempts to stand up on its own. This forms the basis for a physical character rig i'm working on.



I recently included the option for the pivot to lean in a particular direction that is controlled by a Quaternion "targetDir".

This works fine, except the formula I'm using tends to over-correct since it doesn't account for gravity.

https://i.gyazo.com/b1329926f6fd12dbaddf47a3ab7931c3.mp4

I need to change this line of code;

Code (CSharp):

1. float idealSpeed = Mathf.Sqrt(2 * acceleration * Mathf.Abs(distance)) * Mathf.Sign(distance);

So that it includes the effects of gravity within the calculation.

I've included a relevant portion of the source code below. If anyone is interested I can post the entire thing as a prefab for everyone to use.

Code (CSharp):

1. public Rigidbody rigidBody; //pivothead
2. public float height = 1; //distance from pivothead to base
3. public float acceleration = 3; //maxacceleration of pivothead
4. public Quaternion targetDir = Quaternion.identity; //desired direction for the pivot to lean towards
5. public Rigidbody pivotBase; //rigidbody under the pivothead, essentially a ballsocket joint
6.  
7. float SphericalDistance(Vector3 position1, Vector3 position2)
8. {
9.     return Mathf.Acos(Vector3.Dot(position1, position2));
10. }
11.  
12. private Vector3 localPosition
13. {
14.     get
15.     {
16.         Vector3 local = rigidBody.position - pivotBase.position;
17.         return local;
18.     }
19. }
20.  
21. private Vector3 upVector
22. {
23.     get
24.     {
25.         Vector3 local = localPosition;
26.         local = Quaternion.Inverse(targetDir) * local;
27.  
28.         Vector3 n = new Vector3();
29.         n.x = (Mathf.InverseLerp(-height, height, local.x) * 2) - 1;
30.         n.y = Mathf.InverseLerp(height, 0, local.y);
31.         n.z = (Mathf.InverseLerp(-height, height, local.z) * 2) - 1;
32.  
33.         //if almost standing upright give default answer;
34.         if (Approximately(n.x, 0, 0.0001f) && Approximately(n.y, 0, 0.0001f) && Approximately(n.z, 0, 0.0001f))
35.             return Vector3.forward;
36.  
37.         Vector3 up = new Vector3((1 - n.y) * -n.x, n.y, (1 - n.y) * -n.z).normalized;
38.  
39.         up = targetDir * up;
40.  
41.  
42.         return up;
43.     }
44. }
45.  
46. private Vector3 rightVector
47. {
48.     get
49.     {
50.         Vector3 up = upVector;
51.  
52.         Vector3 right = Quaternion.LookRotation(up) * Vector3.right;
53.  
54.         return right;
55.     }
56. }
57.  
58.  
59. private void ApplyBalanceCorrection()
60. {
61.     float distance = SphericalDistance(targetDir * Vector3.up, localPosition / height);
62.  
63.     float idealSpeed = Mathf.Sqrt(2 * acceleration * Mathf.Abs(distance)) * Mathf.Sign(distance);
64.     float speedForward = Vector3.Dot(rigidBody.velocity, upVector);
65.     float speedRight = Vector3.Dot(rigidBody.velocity, rightVector);
66.  
67.     float forwardDiff = speedForward - idealSpeed;
68.  
69.     float accFixed = acceleration * Time.fixedDeltaTime;
70.  
71.     Vector3 forwardVelAdd, rightVelAdd;
72.  
73.     forwardVelAdd = upVector * ((-Mathf.Sign(forwardDiff)) * Mathf.Min(accFixed, Mathf.Abs(forwardDiff)));
74.     rightVelAdd = rightVector * ((-Mathf.Sign(speedRight)) * Mathf.Min(accFixed, Mathf.Abs(speedRight)));
75.  
76.     Vector3 additionalVelocity = rightVelAdd + forwardVelAdd;
77.  
78.     additionalVelocity = additionalVelocity.Sanitise();
79.  
80.     rigidBody.AddForce(additionalVelocity, ForceMode.VelocityChange);
81. }