Ball-orientation-Nan-fix

VisualPinball.Unity/VisualPinball.Unity/VPT/Ball/BallData.cs

@@ -26,7 +26,43 @@ internal struct BallData : IComponentData
 		public float3 Position;
 		public float3 EventPosition; // m_lastEventPos
 		public float3 Velocity;
+
+		/// <summary>
+		/// AngularVelocity  -  german: Winkelgeschwindigkeit
+
+		///		* Set to 0 at Manual Roll
+		///			(in BallManualRoll(in Entity entity, in float3 targetWorldPosition)
+		///			(which is not used anywhere in this Project, but is at least used in Ravarcade's ImGui Physics Debugger - Addon)
+		///		* Is set to zero At RotatorComponent. Possibly an error and should be AngularVelocity 
+		///			(in UpdateRotation(float angleDeg))
+		///		* Calculated from AngularMomentum / inertia 
+		///			(In BallDisplacementSystem.OnUpdate())
+		///			(Where Inertia is a "constant" based on radius and mass (2/5 m r^2))
+		///		* Used to get tangential velocity due to rotation when rolling / colliding on surfaces (alsways added to normal velocity) 
+		///			(in BallData.SurfaceVelocity(in BallData ball, in float3 surfP))
+		/// </summary>
 		public float3 AngularVelocity;
+		/// <summary>
+		/// AngularMomentum  - german: drehimpuls, Impulsmomemt
+		///		* Set to 0 at Manual Roll
+		///			(in BallManualRoll(in Entity entity, in float3 targetWorldPosition)
+		///		* Set to 0 at every new ball	
+		///			(in Ballmanager.CreateEntity(GameObject ballGo, int id, in float3 worldPos, in float3 localPos, in float3 localVel, in float scale, in float mass, in float radius, in Entity kickerEntity)
+		///		* Set to 0 in KickerApi, KickerCollider and RotatorComponent
+		///			(in several places)
+		///		* Calculated when a survace applies an impulse, it applies it to velocity (div by mass) and to angMom fully. 
+		///			(ApplySurfaceImpulse(in float3 rotI, in float3 impulse))
+		///			(Where rotI seems to be the Rotation impulse and impulse is the (non angular)velocity (makes sense to divide by mass)
+		///			(angularMomenmtom = rotI;)
+		///		* used to calculate Angular Velocity (ball.AngularVelocity = ball.AngularMomentum / inertia;)
+		///			(in BalldisplacementSystem.OnUpdate())
+		///		* used to add and thus calculate Orientation 
+		///			(in BalldisplacementSystem.OnUpdate())
+		///			skewSymmetricMatrix is created from the Angular Momentum divided by Inertia
+		///			The original orientation is multiplied with the skewSymmetric Matrix
+		///			and added to the old Orientation to form new orientation
+		///			
+		/// </summary>
 		public float3 AngularMomentum;
 		public float3x3 Orientation;
 		public float Radius;
@@ -72,7 +108,12 @@ public float CollisionRadiusSqr {
 			}
 		}
 
+		/// <summary>
+		/// Calculates Moment of Inertia for a Ball 
+		/// https://en.wikipedia.org/wiki/Moment_of_inertia#Examples_2
+		/// </summary>
 		public float Inertia => 2.0f / 5.0f * Radius * Radius * Mass;
+
 		public float InvMass => 1f / Mass;
 
 		public void ApplySurfaceImpulse(in float3 rotI, in float3 impulse)

VisualPinball.Unity/VisualPinball.Unity/VPT/Ball/BallDisplacementSystem.cs

@@ -1,4 +1,4 @@
-// Visual Pinball Engine
+// Visual Pinball Engineball.Orientation
 // Copyright (C) 2022 freezy and VPE Team
 //
 // This program is free software: you can redistribute it and/or modify
@@ -57,6 +57,37 @@ protected override void OnUpdate()
 				ball.Orientation += addedOrientation;
 				math.orthonormalize(ball.Orientation);
 
+				// after Orthonormalization, the orientation vectors also have to be normalized - this is not done by othonomalize, since the skew matrix creates quite lengthy vectors.
+				// in fact, they dont have to be normalized, but just shortened, so we can abs-add the x, y and z together and just divide by the sum.
+				// This saves three sqrts in the game loop per ball. 
+				float lengthX, lengthY, lengthZ;
+				/* Correct normalization would be: 
+				 * lengthX = math.sqrt(ball.Orientation.c0.x * ball.Orientation.c0.x + ball.Orientation.c0.y * ball.Orientation.c0.y + ball.Orientation.c0.z * ball.Orientation.c0.z);
+				 * lengthY = math.sqrt(ball.Orientation.c1.x * ball.Orientation.c1.x + ball.Orientation.c1.y * ball.Orientation.c1.y + ball.Orientation.c1.z * ball.Orientation.c1.z);
+				 * lengthZ = math.sqrt(ball.Orientation.c2.x * ball.Orientation.c2.x + ball.Orientation.c2.y * ball.Orientation.c2.y + ball.Orientation.c2.z * ball.Orientation.c2.z);
+				 */
+				lengthX = math.abs(ball.Orientation.c0.x) + math.abs(ball.Orientation.c0.y) + math.abs(ball.Orientation.c0.z);
+				lengthY = math.abs(ball.Orientation.c1.x) + math.abs(ball.Orientation.c1.y) + math.abs(ball.Orientation.c1.z);
+				lengthZ = math.abs(ball.Orientation.c2.x) + math.abs(ball.Orientation.c2.y) + math.abs(ball.Orientation.c2.z);
+				if (lengthX != 0f)
+				{
+					ball.Orientation.c0.x /= lengthX;
+					ball.Orientation.c0.y /= lengthX;
+					ball.Orientation.c0.z /= lengthX;
+				}
+				if (lengthY != 0f)
+				{
+					ball.Orientation.c1.x /= lengthY;
+					ball.Orientation.c1.y /= lengthY;
+					ball.Orientation.c1.z /= lengthY;
+				}
+				if (lengthZ != 0f)
+				{
+					ball.Orientation.c2.x /= lengthZ;
+					ball.Orientation.c2.y /= lengthZ;
+					ball.Orientation.c2.z /= lengthZ;
+				}
+
 				ball.AngularVelocity = ball.AngularMomentum / inertia;
 
 				marker.End();

VisualPinball.Unity/VisualPinball.Unity/VPT/Ball/BallMovementSystem.cs

@@ -58,11 +58,23 @@ protected override void OnUpdate()
 
 				// calculate/adapt height of ball
 				var zHeight = !ball.IsFrozen ? ball.Position.z : ball.Position.z - ball.Radius;
-
-				var or = ball.Orientation;
 				var ballTransform = _player.Balls[entity].transform;
 				ballTransform.localPosition = new Vector3(ball.Position.x, ball.Position.y, zHeight);
 
+
+				var or = ball.Orientation;
+				var vpright = new Vector3(or.c0.x, or.c1.x, or.c2.x);
+				var vpfront = new Vector3(or.c0.y, or.c1.y, or.c2.y);
+				var vptop = new Vector3(or.c0.z, or.c1.z, or.c2.z);
+				// Debug.Log("c0: (" + or.c0.x + ", " + or.c0.y + ", " + or.c0.z + ")");
+				// Debug.Log("c1: (" + or.c1.x + ", " + or.c1.y + ", " + or.c1.z + ")");
+				// Debug.Log("c2: (" + or.c2.x + ", " + or.c2.y + ", " + or.c2.z + ")");
+				Vector3.OrthoNormalize(ref vptop, ref vpfront, ref vpright);
+				var unitytop = new Vector3(vptop.x, vptop.z, vptop.y);
+				var unityfront = new Vector3(vpfront.x, vpfront.z, vpfront.y);
+
+				Vector3.OrthoNormalize(ref unitytop, ref unityfront);
+
 				// Following is the transistion from VP-Physics Ball Orientation to the Unity Ball-Orientation.
 				// following statements: when looking at the backglass:
 				// The problem here is, that we have 
@@ -81,7 +93,17 @@ protected override void OnUpdate()
 				//1st iteration by (looks strange, but less strange)  (cupiii)
 				//ballTransform.localRotation = Quaternion.LookRotation(new Vector3(or.c0.x*-1, or.c1.x*-1, or.c2.x), new Vector3(or.c0.z*-1, or.c1.z*-1, or.c2.z));
 				//newest iteration (hopefully correct))
-				ballTransform.localRotation = Quaternion.LookRotation(new Vector3(or.c0.z*1f, or.c2.z*1f, or.c1.z*1f), new Vector3(or.c0.y*1f, or.c2.y*1f, or.c1.y*1f));
+
+				// Better Ways than skew matrix: 
+				//    https://gamedev.stackexchange.com/questions/108920/applying-angular-velocity-to-quaternion
+				//	  https://stackoverflow.com/questions/23503151/how-to-update-quaternion-based-on-3d-gyro-data
+				//    https://stackoverflow.com/questions/12053895/converting-angular-velocity-to-quaternion-in-opencv
+
+				// also implementing VP's "Orthonormalize" Code - although it does not really orthonormalize could give a performance benefit:
+				// https://github.com/vpinball/vpinball/blob/be08b04d61096272df97bd45e6f0682043228a73/math/matrix.h#L208
+
+				ballTransform.localRotation = Quaternion.LookRotation(unityfront, unitytop);
+
 
 				marker.End();