Resolved Rotating force around pivot point

So i have been working on this issue for a while and I'm just stuck. I have 2 objects at (0,0,0) except they are rotated at different degrees.

If I have ObjectA that has a Euler rotation of (0,0,0) and ObjectB has a Euler rotation of (0,90,0) then I use this code to get a relative force on B to be enacted on ObjectA

Code (CSharp):

1.  
2. public static float3 RotateAround(float3 position, float3 pivotPoint, quaternion rot)
3. {
4.     return math.mul(rot, (position - pivotPoint)) + pivotPoint;
5. }
6.  
7. /// <summary>
8. /// Rotates a transform around the given Pivot point by the given rotation.
9. /// </summary>
10. /// <param name="transform"></param>
11. /// <param name="pivotPoint"></param>
12. /// <param name="rot"></param>
13. /// <returns></returns>
14. public static LocalToWorld RotateAround(LocalToWorld transform, float3 pivotPoint, quaternion rot)
15. {
16.     return new LocalToWorld
17.     {
18.         Value = new float4x4(math.mul(rot, transform.Rotation),
19.             math.mul(rot, (transform.Position - pivotPoint)) + pivotPoint
20.         )
21.     };
22. }
23.  
24. /// <summary>
25. /// Rotates a transform around the given Pivot point by the given angle on the given axis.
26. /// </summary>
27. /// <param name="transform"></param>
28. /// <param name="pivotPoint"></param>
29. /// <param name="axis"></param>
30. /// <param name="angle"></param>
31. public static void RotateAround(Transform transform, Vector3 pivotPoint, Vector3 axis, float angle)
32. {
33.     Quaternion rot = Quaternion.AngleAxis(angle, axis);
34.     transform.position = rot * (transform.position - pivotPoint) + pivotPoint;
35.     transform.rotation = rot * transform.rotation;
36. }
37.  
38. /// <summary>
39. /// Rotates a transform around the given Pivot point by the given angle on the given axis.
40. /// </summary>
41. /// <param name="transform"></param>
42. /// <param name="pivotPoint"></param>
43. /// <param name="axis"></param>
44. /// <param name="angle"></param>
45. /// <returns></returns>
46. public static LocalToWorld RotateAround(LocalToWorld transform, float3 pivotPoint, float3 axis, float angle)
47. {
48.     quaternion rot = quaternion.AxisAngle(axis, angle);
49.     return new LocalToWorld
50.     {
51.         Value = new float4x4(
52.             math.mul(rot, transform.Rotation),
53.             math.mul(rot, (transform.Position - pivotPoint)) + pivotPoint)
54.     };
55. }
56.  
57. //NOTE ObjectB has a float3 position and a quaternion rotation
58.  
59. float3 m_lastCalculatedForce = (ObjectB.position - ObjectB_oldPosition);
60.  
61. float forceMagnitude = Unity.Mathematics.math.length(p.m_lastCalculatedForce);
62. float3 unitForceVector = p.m_lastCalculatedForce / forceMagnitude;
63. float3 newVector = TransformsExtensions.RotateAround(unitForceVector, float3.zero, math.inverse(ObjectB.rotation));
64.  
65. Debug.Log(p.m_lastCalculatedForce + "::" + forceMagnitude + "\n" +
66.     "::" + unitForceVector + ":::" + math.length(unitForceVector) + "\n"
67.     + newVector + ":::" + math.length(newVector));
68.  
69. p.m_lastCalculatedForce = newVector * forceMagnitude;

This works only on the Y axis. the X and X rotation axis somehow messes everything up.
Meaning i ObjectB is (90,0,0) and thie code runs then the force is in the wrong direction.

Any ideas?

 

I figured it our however another problem arose. here is the code i used if anyone is interested

Code (CSharp):

1.  public static Vector3 RotateDirectionVector(Vector3 direction, Vector3 rotation)
2.         {
3.             direction.Normalize();
4.             Quaternion rot = Quaternion.Euler(rotation); // Rotate [angle] degrees about the x axis.
5.             direction = rot * direction;
6.             return direction;
7.         }