Rotating with AddForceAtPosition causing Gimbal lock-like behavior

I have a GameObject which is acting as the parent GameObject to several other objects.
The parent is the only GameObject with a RigidBody attached to it.
Each child has the potential to add force to the parent RigidBody in order to move the overall object in some arbitrary direction.
The idea is that a force vector and magnitude are calculated in the child, then applied to the parent RigidBody to cause some sort of movement.
I'm using AddForceAtPosition in order to make it feel like the force is being applied at the child object, since the children don't have their own RigidBody component.
Below is the FixedUpdate from the child script (Some changes have been made to facilitate readability on this site).

Code (CSharp):

1.  
2.     public void FixedUpdate()
3.     {
4.         // Find velocity relative to main rigid body
5.         this.localVelocity =
6.            this.ParentRigidBody.GetPointVelocity(this.transform.position);
7.         // Find vector perpendicular to local velocity,
8.         //  in the direction of local up.
9.         this.forceVector = Vector3.Cross(
10.           Vector3.Cross(this.localVelocity, this.transform.up),
11.           this.localVelocity);
12.         this.forceVector.Normalize();
13.    
14.         // Find signed angle.
15.         var theta1 = Vector3.Angle(this.localVelocity, this.transform.up);
16.         var theta2 = Vector3.Angle(this.localVelocity, this.forceVector);
17.         this.Angle = theta1 - theta2;
18.        
19.         // Find speed squared
20.         var speed = Vector3.Dot(this.localVelocity, this.transform.forward);
21.         var sqrSpeed =  Mathf.Pow(speed, 2);
22.         // Calculate magnitude
23.         // ForceTable.GetCoefficient takes a float 'angle' which it uses
24.         // as the key in a key/value pair lookup.
25.         // In general, positive angles will return positive
26.         // coefficients, and negative angles will return negative coefficients.
27.         // Coefficients are small [-1, 1], more or less.
28.         var coefficient = ForceTable.GetCoefficient(this.Angle);
29.         // this.InputOffset is, as it may sound, calculated based on user input.
30.         // The value is pretty much always either -0.14f, 0f, or 0.14f
31.         coefficient += this.InputOffset;
32.         this.magnitude = 0.5f * sqrSpeed * this.Area * coefficient;
33.         // apply force.
34.         this.ParentRigidBody.AddForceAtPosition(
35.           this.forceVector * this.magnitude,
36.           this.transform.position);
37.     }

The issue I'm having revolves (HA!) around rotation. In this case, I have two children objects, each placed on opposite sides of the parent's center of mass, with symmetry across the parent's local forward vector.

If I apply positive input to one child, and negative to the other, I would expect the overall object to rotate to the right or left.

For the most part, this works as expected.

The issue comes once the object hits 90 degrees (or -90 degrees) rotation, at which point the object stops rotating around the local forward vector, and begins rotating around the local left/right vector (now aligned with world 'up'). Normally I would scream 'gimbal lock!' and run about waving Quaternions, but since I'm not actually calculating and applying rotations, I feel like this is an unlikely culprit.

Any thoughts or insights would be greatly appreciated, and thank you.

 

This one might be a problem:

Code (csharp):

1.  
2. this.forceVector = Vector3.Cross(
3.           Vector3.Cross(this.localVelocity, this.transform.up),
4.           this.localVelocity);
5.  

When this.localVelocity and this.transform.up are aligned nearly in the same direction, the perpendicular vector you're computing with that inner cross product can go haywire and not really be in the direction you wanted. If this is the case, do a dot product on those two first, if they're a little too close to 1 or -1 (vectors pointing in nearly the same or opposite directions), choose a different vector instead of this.transform.up. One of the other two direction vectors should be ok. Just be sure in the end you get the force in the same direction you were after originally somehow.

What I usually do is render the forces and direction vectors so I can see exactly what they're all doing. Gimble lock isn't going to happen in a physics sim. If you get something like it, chances are the forces are going in the wrong directions which can look similar to gimble lock. A couple of Debug.DrawLine() statements will probably show you exactly where things go wrong.

 

Actually, if you wanted to check it really quickly to see if it's really the issue, you could do a public float dot of that inner dot product (so you can see it in the inspector) and see if it gets really close to 1 or -1 at the near 90 degree rotations where it starts doing funny things.

I.e., just do this:

Code (csharp):

1. dot = Vector3.Dot(this.localVelocity, this.transform.up)

I've run into similar issues in my own physics engine. Not gimble lock type stuff, but vectors that end up pointing in directions that they shouldn't. The solution for me has always been to just pick a different vector (right or up or a world aligned vector or something), and in the future be more cautious about my vector choices. Can't always get around it though. Sometimes you just have to do an "if" somewhere.

 

I ended up scrapping what I had and trying something a little different. Instead of having one master object/rigidbody combo and a bunch of child objects trying to act on it, each part has its own rigidbody through which it acts.

I had to spend some time figuring out how to glue all the parts together at run time, but with a little bit of ray tracing, I was able to create configurable joints to keep all the parts attached where I needed them.

From there, I calculate center of mass for each part (mostly just center of mass = position), and each part that is responsible for adding physics forces does so through its own center of mass. Everything seems to be quite stable now!

Thanks for your inputs, they were very valuable, even if I didn't end up going down that road after all.

 

Good to hear that you solved the problem.

 

The interesting part about the fact that my new direction fixed my issue is that I think I was assuming too much about the original problem. My new theory is that I was misusing AddForceAtPosition -- or at least miscalculating the force vector and position. I think I didn't quite do the world->local and local-> world calculations correctly.

 

It's really easy to make mistakes, for sure. This is why I usually draw all the force vectors at their positions just to make sure. I'm making a speedboat simulator that has something like 1500 forces on the boat. Besides looking pretty cool when you Debug.DrawLine all of them, and it becomes immediately obvious when a mistake is made. I also take the time to make booleans in the inspector so I can turn on and off the debug rendering of various forces, so I can look at any combination of aerodynamic/hydrodynamic forces, skin friction forces, and buoyancy forces.

I'm glad it's working out for you. If you need any help or advice, drop me a note. I spent 14 years writing the physics engine for VRC Pro, so have figured out a few things along the way.

 

Wow, thank you! That is exceptionally generous of you!

And yeah, what was throwing me off the most, I think, was the fact that I was drawing the force lines. The only thing was, I was using Debug.DrawRay with the origin point as the center of the child object, and the direction as the normalized force vector -- so it would always look like it was doing the right thing.

I really like the idea of a bunch of bool values to turn on and off the different forces -- my solution is slightly different. Each force is being managed by its own script, so everything that can create drag has DragBody attached, everything that can provide lift has LiftBody, and similar for thrust.
Then I draw my forces in OnDrawGizmos . This lets me turn on and off all the forces at the gizmos level, and each force for each part by expanding or collapsing the script component in the inspector panel.

 

Interesting approach. I haven't played with OnDrawGizmos yet. In my case it probably wouldn't work because the forces don't exist until the boat is doing something. I.e., when the game is running. I'm doing three forces at every polygon in the boat, so in my case it works best to have it all done in one script. Probably I'll bung it over to a compute shader because I'm already using about 50% of the Unity thread just to compute the forces on one boat.

 

My forces work similarly, in that things need to be moving around for them to happen (can't get much lift with a velocity of 0, for example). OnDrawGizmos works in the editor and in play(debug) mode, though, so it makes for a convenient way to just turn things off. I find putting everything in there to be of limited use, really, since I so rarely want to toggle force lines all at the same time. Different strokes, as they say.

Also, calculating each force for each polygon sounds like it would give you an incredible amount of realism... but also sounds like it requires a bit more understanding of the physics than I currently have. I'm just looking up drag and lift coefficients in a big ole table based on angle of attack and plugging that into a basic lift formula (0.5f * rho * lift area * lift coefficient * v^2). I only have a few tables, since I'm still prototyping, but at least they are based on NACA airfoil shapes, so that's cool.

Moving force calculations into a shader sounds like an extremely good idea!

 

1/2 rho, etc.. That's pretty much what I'm doing too for hydrodynamic and aerodynamic forces. I simplify it though by taking that in the direction normal to the face. The upside is that it's simple and fast, the downside is that the drag and lift coefficients that come out of it are purely geometrical in nature and can't be tuned. If you're doing airfoils (flight sim, by chance?) it'd no doubt be more accurate to do it your way, especially since you can just tune it to NACA data.

For a boat, my way worked surprisingly well. My surfaces are mostly enclosed by the vee-shape of the hull, all triangles have other triangle surfaces adjoining them. I throw out some of them depending on velocity versus normal direction so the back of the boat doesn't drag through the water and other such things. Just yesterday I tried doing the propeller the same way and it worked great. The boat turns totally differently now (much harder and more stable, a lot more like a real speedboat). It also trims a lot more like a real boat because the blade angles cause off-axis forces that depend on the angle of attack of the prop. Every blade is then at a different angle of attack and velocity and changing as it spins. It looks pretty crazy when you plot it while the game's running.

I also split the triangles at the waterline so triangles can be partly submerged. I.e., any submerged triangles get split into three sub triangles, one above and two below the waterline (or vice versa, two above and one below). So I get a real nice bow lifting effect where you can run the boat just on the pad at high speeds. Go too fast and it'll take off and blow over like an unstable airplane, which sends you back to the hull designer part of the game to try and shape that out of the boat somehow. With the new prop stuff it also twists to the right and drops the nose when you come out of the throttle fast. All very much like a real speed boat. It'll chine walk to a large degree too, basically all the nasty things that can happen in a real boat can happen here. If the prop is partly submerged (surface piercing) it'll paddle wheel the bottom half of the prop which twists the boat. It's fun. I grew up on a lake where my family had a 60mph+ outboard. It's the first time I've experienced anything like it in a boat sim. Usually they're pretty arcade and bland, nothing interesting happens.

I started a thread recently about it. Seems nobody here cares, but there are some vids there anyway if you're curious. Those are all from before the new prop model. One of them shows the waterline mesh splitting:

http://forum.unity3d.com/threads/oculus-rift-dk2-speedboat-simulator.271738/

Compute shader: Yes, I think this would work very well. In my case all of the polygons are completely independent from each other (flow over one polygon does not affect another), so it'd be perfect for a compute shader. The only trouble is the mesh splitting may not work very well because there are a lot of "if" statements in doing that which GPUs don't really like. Still though, I might take a look at that. If it's fast enough then I could just crank up the resolution on the mesh enough for the mesh splitting not to really matter anymore anyway. Brute force it to simplify everything. Not sure, the audio system I wrote is also compute shader driven and is very heavy, so I'm not sure yet what I'll be able to get away with. If nothing else, it would just be cool to be able to say the boat is simulated with 20,000 forces or something like that.

 

Well participating this thread has really given me a confidence boost. I am glad to hear that I am on the right track!

I am indeed working on a flight simulator. I started out with a test plane that uses jet engines, because modeling that type of thrust was simple enough to reduce to a single AddForce call with MaxThrust * current throttle * thrust direction normal.
Just yesterday I started toying with prop engines -- so it's again comforting to know that I'm not crazy in thinking it is a good idea. The nature of my particular flight sim tends to make modeling props difficult, though. Much like you allow users to create a boat hull, I'm allowing users to completely craft their plane -- all the way down to the airfoils of the propellers.
This causes a problem at high rotational speeds, and everything just becomes unstable.

What I'll probably end up doing is giving the user an editor dedicated entirely to propeller prop-engine assembly design. That way I can take whatever contraption they've devised and convert everything that spins into one mesh. That way the props won't buckle or bend at thrust-producing rotational speed. The only issue that presents me with is if the user has multi-part propellers. Since each part should have its own 'wing' component to produce lift / thrust, if I combine them all into one mesh I'll have to come up with another way to model them. Perhaps I really just need a propeller component after all.

As for your project -- that looks really cool. I've not even considered dabbling in the oculus and yours is really the first project I've actually taken the time to look at the video for. I'm impressed I really like the effect the mesh splitting gives you.
You are definitely further along in your project than I am in mine. I'm not quite ready to put it up on the Works in Progress section yet, I still consider it in the 'tech demo / prototype' phase. Maybe once the prototype is "complete" I'll start taking screen shots and videos.
As for people not caring -- I have my own complaints about the Unity community... but that's a whole other thread, I think, lol.

Also, I edited my previous post because I definitely meant 0.5f * rho, not 5 * rho.

 

Sounds like you're doing for planes what I'm doing for boats, pretty much. Neat.

What do you mean? Are you using a rigid body and it's going crazy at high speeds? If so, there is a way around that: Don't use a rigid body at all for the prop and instead compute the physics of that yourself. My prop doesn't care how fast it's turning, it works at a million rpm just fine because there's no rigid body there to blow up via some Unity/PhysX constraint issues on the boat. I handle it only as a 1D object that has a rotational inertia around only one axis, a torque only about one axis, and a single angular velocity about that same, one axis. Because the rotational physics of the prop are 1D, I don't get any gyroscopic torques or precession out of it, so that's a drawback, but it seems to do the trick pretty well anyway.

Granted, in your case if people can build big helicopter rotors and aircraft props and so on that have significant precession/nutation, you can compute the gyroscopic torque yourself approximately and add it to the aircraft rigidbody with the Unity function (SetTorque or whatever it is).

 

Yeah, that is exactly what I mean. Each propeller blade is one or more objects which have 'wing' components on them. Wing components require rigid bodies, since they produce and apply lift individually. Each part of the prop blade is held to the next with a configurable joint, and each full blade is held to the crank shaft similarly, and again for the crank shaft to the engine block -- although the crank shaft -> engine joint allows free rotation in one axis.

This causes all kinds of problems, as you can imagine.
The plan is to pretty much do what you suggest, but I don't want the user to see any difference between a wing and a propeller blade (because, really, there isn't one). So I'll remove the rigid body for each blade segment, but continue to calculate forces as I am already doing. Then I'll just apply the forces at the next closest rigid body -- the engine.

We'll see how that goes!

 

Sounds like a plan. If that doesn't work, perhaps you could lose the rigid body on the engine too and just keep the whole aircraft as one body. Quicker computationally anyway, constraints between rigid bodies are bound to be expensive.

When you get around to posting something in the new projects/whatever thread, drop me a note. It'd be neat to see what you're doing

 

http://forum.unity3d.com/threads/oculus-rift-dk2-speedboat-simulator.271738/#post-1802295


Here's what mine looks like with all the forces turned on: You can see the individual prop blade forces too. Works quite well and it made a big difference to how the boat drives and responds to trim changes. Definitely was worth doing.

 

Something I just realized: Some of the functions like Vector.Cross (and probably others) are really slow compared to hand computing it. I just got a function running something like 40 times faster by hand coding it with all the x's and y's and z's done separately. Worth a look if you get to doing lots of forces...

 

Turns out, my actual problem was a combination of too-low timestep and doing something silly each frame. I was storing lift and drag coefficients, then accessing them based on the specific angle of attack. I thought I was using a dictionary to do this, but it turns out I was actually making a sql call for each part, per frame, per force. Oops!
I tweaked the time step and fixed my design to *actually* access the coefficients in a more efficient manner, and now I have a prop engine!

The problem now is that I've decided that my model to determine lift/drag is ineffective for what I want. I cannot assume that each wing is going to be based on a NACA 4 series of airfoil (which I am currently doing, since lift/drag data for these is easy to find). This means I need to figure out a better/different way to determine lift and drag per part at an arbitrary angle of attack.

I'm definitely in the weeds right now, and I'm not sure how I'm going to continue.

 

Congrats on the prop! Good to hear

Maybe I'm wrong, but I'd think for a game you'd be able to get away with just one airfoil or two. Do you really imagine players experimenting with libraries full of them rather than just tweaking the wing span, aspect ratio, etc., a bit, and the NACA 4 airfoils not being enough to play with? I don't really know what you have in mind though, so maybe it's something you really have your heart set on. I wouldn't let the possibility that it may be impractical to import every single airfoil file ever made get you down though. You could always add more later if players aren't satisfied with the (hundreds/thousands?) that come with the NACA 4 data.

I spent the last couple days rewriting a lot of my boat code. I've separated things into another class that can just be dropped onto any mesh in a game and it'll do fluid dynamic/buoyancy/skin friction forces and so on. The rigid body can belong to that object or another object can be dragged onto it. Spent most of yesterday optimizing things and have managed to get the physics code running 1500% faster, maybe better. It looks like the way to go is to avoid using the Vector3 class and even a lot of the unity rigid body functions for math ops. Even doing a simple assignment or addition like this:

Code (csharp):

1.  
2. Vector3 firstVector;
3. Vector3 secondVector;
4. Vector3 thirdVector;
5.  
6. thirdVector = firstVector; //Assignment
7. thirdVector = firstVector + secondVector; //Addition
8.  

....is significantly slower (maybe a factor of 10 or more) than doing this:

Code (csharp):

1.  
2. thirdVector.x = firstVector.x;
3. thirdVector.y = firstVector.y;
4. thirdVector.z = firstVector.z;
5.  
6. thirdVector.x = firstVector.x + secondVector.x;
7. thirdVector.y = firstVector.y + secondVector.y;
8. thirdVector.z = firstVector.z + secondVector.z;
9.  

I found the same to be true for all of the Vector3 methods tested like magnitude, cross, dot, normalized, and so forth. I've replaced all the GetPointVelocity and TransformPoint functions with my own functions that process arrays full of vectors and the speed improvements are truly massive in many cases. In deep profile mode I got one function to go from 0.63ms all the way down to 0.07ms. Another one I got down to ~0ms, so fast I had to loop it 100 times just to see what it was really doing.

From now on I'll be using the Vector and some of the Unity methods only for prototyping, then replacing them with hand written code later. It's so much faster to just do the math without any method calls. Now I just do a handful of method calls per mesh regardless of how many triangles there are in it instead of trying to do GetPointVelocity or TransformPoint on every single one. In one part I'm computing the forces and torques directly, adding them all up, then applying only one resultant force and torque to the rigid body. Massive increases in speed for sure.

I want to try to do this in local coordinates. That might get rid of a lot of the transform computations and make it really fly.

I wish C# did function macros. Those are just text replacements that act like functions without any actual function call being done. Then we'd have the best of both worlds: Easy programming and super fast execution speed.

 

Another little speed trick I discovered. When you want absolute value normally you'd do this:

Code (csharp):

1. float myAbsoluteValue = Mathf.abs(myNumber);

Oddly enough, it was a lot faster to do this:

Code (csharp):

1.  
2. float myNumber = whatever;
3. float myNumberSquared = myNumber * myNumber;
4. float myAbsoluteValue = Mathf.sqrt(myNumberSquared);
5.  

Who would have though squaring a number, then taking the square root of it, would be faster than just taking the absolute value?

 

Wow, I can't believe (read: I can totally believe) that Unity's Mathf.abs is slower than squaring and taking the square root. It is just so absurd. I wonder where multiplying by the result of Mathf.sign fits in.

On a related note, I was poking around in some code someone else wrote, and it struck me as add that it looked like they wrote their own vector 3 class. It makes sense, though, if Unity's is that much slower than doing it by hand. Especially for a physics-heavy simulator/game.

I think using the full suite of NACA 4 airfoils is a good fallback plan, in fact it is my fallback plan. The original idea was to give the user much more control and room to play around than limiting them to the 4 series would allow. That being said, there are an awful lot of airfoils in that family, so it is only a limit in the strictest sense of the word.
If I can't make it work the way I want it to, I may just restrict how much tweaking the user can do to an airfoil, then see which NACA 4 it is closest to, and use the data from that.

I'm glad to hear your optimizations are going so well, that is an impressive number. I've heard Unity hides GetComponent calls behind a lot of their seemingly simply accessors (I'm looking at you, rigidbody). Perhaps the performance hits you are seeing related to rigid body have to do with that? I'm currently working with Unity Free, so I can't do any sort of useful profiling at the moment, but I'm also so early in the life of this project that it almost doesn't matter.
Except for when I think my physics calculations are the problem and really its that I'm making sql calls multiple times per frame. Then maybe the profiler would have saved me a week of spinning my wheels.

Edit: I looked at that other guy's code again. It turns out not only did they do their own vector class, but it's a vector of doubles, instead of floats. In fact, most of the code operates in doubles instead of floats. I wonder what the speed / accuracy trade off is for them...

 

GetComponent: Yes, somebody told me today that the transform class is just a wrapper around the GetComponent method. I'm not caching the transform and using that. Eliminating thousands of transform this and that calls undoubtedly had an enormous impact.

Regarding the Vector class stuff, I'm not sure if this is a Unity thing or if it's a Microsoft thing. I tried an unsafe method for abs somebody sent me and it was only a couple of percent faster than using the square-square root trick. If I understood the code correctly (entirely possible that I didn't), that method was literally just flipping a bit to 0 to make the value positive. So I'm not ready to blame Unity for that without really knowing for sure. There's got to be a lot of overhead just calling the method itself even if it was empty.

Doubles versus floats: I haven't investigated this much in a few years. Last time I tried it (PowerBasic compiler) doubles were a bit slower. Nowadays I don't know. Either way, I tend to just stick with floats.

A physicist friend of mine who also wrote a vehicle dynamics engine also wrote all his own classes and overloads. He used SSE instructions with unions to truly vectorize everything. I.e., adding two Vector3s together computed all three components simultaneously. He reported some major speed increases by paying a lot of attention to that.

In my case I also rewrote things to effectively use a structure of arrays instead of an array of structures. I have a "physics triangle" structure (really it's a class, but works the same way) which has a vert0 array, vert1 array, and vert2 array for the triangle verts, a face normal array, and some other stuff. Then I wrote my own TransformPoint and TransformDirection functions that get passed a Vector3 array (vert0 array or a vert1 array or whatever), and the whole array is done in one function call instead of doing bazillions of TransformPoint or TransformDirection calls. One of the related functions I tested today is now 80 times faster than it was before by going in and really hand coding all the math myself. Most of the stuff was at least 15 or 20 times faster, really serious increases in speed. Some of that is probably because all the data is together in one array accessed by one function call, so it probably sits in L1/L2 cache where it can really fly. Data oriented design.

Some of the improvement is probably because of the elimination of thousands of method calls per frame. GetComponent probably isn't helping matters. Anyway, I've developed a bunch of new habits over the last two days for sure. Next up is to do it on my mesh splitting function which has lots of horrible things in it. It won't be surprising if this one is 100 times faster when it's done.

I've never used SQL. All I know is it's a database thing. Are you just using it for reading the NACA data?

 

Yeah, I have my data in excel sheets currently, so the quickest thing I could think of was to parse that data into a c# DataTable and go from there. There are simply too many values to write them all by hand into something like a dictionary. Going forward, I am going to re-evaluate my stance on that, but for now I just read the values once per airfoil and store them in a dictionary. That way when I need the info for a specific airfoil / angle of attack combination, it is just a quick dictionary lookup.