How to find position in rigid body dynamics simulation using Kinetic Energy - math

I am trying to implement rigid bodies dynamics for my simulation. I am trying to simulate a tensegrity as in the paper attached.
The paper I follow is: Dynamic equations of motion for a 3-bar tensegrity based mobile robot
According to this paper and what I have read so far the general process is:
For each bar:
A) define the bar’s pose (q) : for that I need a position vector and an
orientation vector. I have this since it is my initial position.
B) Apply initial velocity: which is 0
C) Find Kinetic Energy
D) Find Potential Energy
E) Find Torque
For the total system:
A) Find Kinetic Energy
B) Find Potential Energy
C) Find Torque
My question is how can I find the new pose (position and orientation) of each bar according to Kinetic Energy, Potential Energy and Torque?
As I understand, I will need the linear acceleration and angular acceleration, and then the linear velocity and angular velocity and finally update pose (position and orientation). I don’t understand how could I do this.
I looked at those books and read some notes, but I didn't understand practically how to do this
Rigid Body Dynamics Algorithms, by Roy Featherstone
Physics for Game Developers, David M.Bourg
Physically Based Modeling, David Baraff
Extra Clarification if possible:
1) Basically the W matrix that requires the angles of φ, θ, would those angles be the angles of the basic reference frame for the whole structure? Not the angles for each bar, because this is not , I assume it is for the whole system?
2) Regarding the Inertia matrix of the mobile reference frame, I understand this must refer for each bar? But since is for the whole system, what should be the matrix of ?
3) How to factorize M(q)
4) In the paper, it is described through in the cables. I would like to try something else, the bars connected together in the center and each one would be possible to move axially only. So I guess I will have to add a force for each bar and affect this?
If you have any information could you please let me know?

The answer to your question is equation 12 in your paper, which gives you the acceleration at time t.
Computing acceleration
If you look carefully at equation 12, every term is previously defined (except maybe the gravitational forces which are -g * mi along the z axis for each node i, thus f gi = [0, 0, -g * mi]). I recommend that you start building/computing each of these terms (taking into account the remarks on changing basis for I).
To differentiate M(q), remember that all of the mi, li, r1i and r2i are constant, thus only the W have a non-zero derivative:
Then just differentiate each term inside the
Only the unknown will remain, which is the acceleration, written in the paper as
You now need to put all the elements that you have computed on the right hand side of the equation, compute the resulting vector, and factorize your M(q), which is the block-diagonal matrix of all the Mi(q), thus this is equivalent to factorizing each Mi(q). Then you will have the acceleration, from which you can integrate velocity, and in turn, position.
Integrating
This is left out of the paper, but isn't too hard. Since you need velocities to compute acceleration, you will have to use a first order integrator like the Semi-Implicit Euler method for example.
Thus the velocity and position should be updated as follows:

Related

Rectangle physics in 2D. Am I doing this right?

I'm writing a 2D game, in which I would like to have crate-like objects. These objects would move around, like real crates do. I have a hypothetical idea of how I would like to achieve that:
Basically I'd store the boxes' corners' coordinates with their force and velocity unit vectors, and in every update I'd basically do the following steps:
1. Apply the forces(gravity, from collisions, etc..) accordingly.
2. Modify velocity vector based on the force.
3. Move every corner of the box, like so:
4. I repeat nr 3. for every corner, so I get the real movement of the cube.
My questions are: Is this approach heading in the right direction? Is this theory even correct? If not, what would be the correct way to move a box around based on vectors in a 2D environment?
Just to clarify: I'm only dragging corner "A" in the picture, but I want to repeat the dragging for every other corner, with their own vectors. By "dragging" I mean the algorithm I just stated.
Keeping each corner's coordinate and speed makes no sense as you would be storing lots of redundant information. Boxes are rigid objects, which means that there are constraints that must be satisfied at any time instant, namely the distance between any two given corners is fixed. This also translates to a constraint that links the velocities of all four corners and so they are not independent values. With rigid bodies the movement of any point is the sum of two independent movements - the linear movement of the centre of mass (CM) and the rotation around a fixed axis - often, but not always, chosen to be the one that goes through the CM. Hence you only need to store the position and the velocity of the crate's CM (which coincides with the geometric centre of the crate) as well as the angle of rotation and the rate of rotation around the CM.
As to the motion, the gravity field is a constant vector field and hence cannot induce rotation in symmetric objects like those rectangular crates. Instead it only produces accelerated vertical motion of the CM. This is also what happens due to all external forces - one has to take their vector sum and apply it to the CM. Only external forces whose direction does not go through the CM give torque and so cause rotation. Such forces are any external pushes/pulls or reaction forces that arise when crates collide with each other or hit the ground / a wall. Computing torque due to external forces is easy but computing reaction forces could be quite involving process because of the constrained dynamics that has to be employed. Once the torque has been computed, one has to divide it by the moment of inertia of the create in order to get the angular acceleration. Often it is more convenient to use another axis and not the one that goes through the CM - Steiner's theorem can be employed in this case in order to compute the moment of inertia around that other axis.
To summarise:
all forces, acting on the create, are first added together (as vectors) and the resultant force (divided by the mass of the create) determines the linear acceleration of the CM;
the torque of all forces is computed and then used to determine the angular acceleration around a given axis.
See here for some sample problems of rigid body motion and how the physics is actually worked out.
Given your algorithm, if by "velocity vector" you actually mean "the velocity of CM", then 1 would be correct - all corners move in the same direction (the linear motion of the CM). But 2 would not be always correct - the proper angle of rotation would depend on the time the torque was applied (e.g. the simulation timestep), and one has to take into account that the lever arm length changes in between as the crate rotates.

Calculating thrusts for offset thruster positions given arbitrary thrusters at arbitrary position around a mass

I've had a bit of a sniff around google for a solution but I believe my terminology is wrong, so bear with me here.
I'm working on a simple game where people can build simplistic spaceships and place thrusters willy nilly over the space ship.
Let's call say my space ship's center of mass is V.
The space ship has an arbitrary number of thrusters at arbitrary positions with arbitrary thrust direction vectors with an arbitrary clamp.
I have an input angular velocity vector (angle/axis notation) and world velocity (vector) which i wish the ship to "go" at.
How would I calculate the the ideal thrust for each of the thrusters for the ship to accelerate to the desired velocities?
My current solution works well for uniformly placed thrusters. Essentially what I do is just dot the desired velocity by the thrusters normal for the linear velocity. While for the angular velocity I just cross the angular velocity by the thrusters position and dot the resulting offset velocity by the thrusters normal. Of course if there's any thrusters that do not have a mirror image on the opposite side of the center of mass it'll result in an undesired force.
Like I said, I think it should be a fairly well documented problem but I might just be looking for the wrong terminology.
I think you can break this down into two parts. The first is deciding what your acceleration should be each frame, based on your current and desired velocities. A simple rule for this
acceleration = k * (desired velocity - current velocity)
where k is a constant that determines how "responsive" the system is. In order words, if you're going too slow, speed up (positive acceleration), and if you're going too fast, slow down (negative acceleration).
The second part is a bit harder to visualize; you have to figure out which combination of thrusters gives you the desired accelerations. Let's call c_i the amount that each thruster thrusts. You want to solve a system of coupled equations
sum( c_i * thrust_i ) = mass * linear acceleration
sum( c_i * thrust_i X position_i) = moment of interia * angular acceleration
where X is the cross produxt. My physics might be a bit off, but I think that's right.
That's an equation of 6 equations (in 3D) and N unknowns where N is the number of thusters, but you've got the additional constraint that c_i > 0 (assuming the thrusters can't push backwards).
That's a tricky problem, but you should be able to set it up as a LCP and get an answer using the Projected Gauss Seidel method. You don't need to get the exact answer, just something close, since you'll be solving it again for slightly different values on the next frame.
I hope that helps...

Quaternion Interpolation w/ Rate Matching

I have an object w/ and orientation and the rotational rates about each of the body axis. I need to find a smooth transition from this state to a second state with a different set of rates. Additionally, I have constraints on how fast I can rotate/accelerate about each of the axis.
I have explored Quaternion slerp's, and while I can use them to smoothly interpolate between the states, I don't see an easy way to get the rate matching into it.
This feels like an exercise in differential equations and path planning, but I'm not sure exactly how to formulate the problem so that the algorithms that are out there can work on it.
Any suggestions for algorithms that can help solve this and/or tips on formulating the problem to work with those algorithms would be greatly appreciated.
[Edit - here is an example of the type of problem I'm working on]
Think of a gunner on a helicopter that needs to track a target as the helicopter is flying. For the sake of argument, he needs to be on the target from the time it rises over the horizon to the time it is no longer in view. The relative rate of this target is not constant, but I assume that through the aggregation of several 'rate matching' maneuvers I can approximate this tracking fairly well. I can calculate the gun orientation and tracking rates required at any point, it's just generating a profile from some discrete orientations and rates that is stumping me.
Thanks!
First of all your rotational rates about each axis should compose into a rotational rate vector (i.e. w = [w_x w_y w_z]^T). Then you can separate the magnitude of the rotation from the axis of the rotation. The magnitude is w_mag = w/|w|. Then the axis is the unit vector u = w/w_mag. You can then update your gross rotation by composing an incremental rotation using your favorite representation (i.e. rotation matrices, quaternions). If your starting rotation is R_0 and your incrementatl rotation is defined by R_inc(w_mag*dt, u) then you follow the following composition rules:
R_1 = R_0 * R_inc
R_k+1 = R_k * R_inc
enjoy.

2D orbital physics

I'm working on a 2D physics engine for a game. I have gravity and masses working, using a simple iterative approach (that I know I'll have to upgrade eventually); I can push the masses around manually and watch them move and it all works as I'd expect.
Right now I'm trying to set up the game world in advance with a satellite in a simple circular orbit around a planet. To do this I need to calculate the initial velocity vector of the satellite given the mass of the planet and the desired distance out; this should be trivial, but I cannot for the life of me get it working right.
Standard physics textbooks tell me that the orbital velocity of an object in circular orbit around a mass M is:
v = sqrt( G * M / r )
However, after applying the appropriate vector the satellite isn't going anything like fast enough and falls in in a sharply elliptical orbit. Random tinkering shows that it's off by about a factor of 3 in one case.
My gravity simulation code is using the traditional:
F = G M m / r^2
G is set to 1 in my universe.
Can someone confirm to me that these equations do still hold in 2D space? I can't see any reason why not, but at this point I really want to know whether the problem is in my code or my assumptions...
Update: My physics engine works as follows:
for each time step of length t:
reset cumulative forces on each object to 0.
for each unique pair of objects:
calculate force between them due to gravity.
accumulate force to the two objects.
for each object:
calculate velocity change dV for this timestep using Ft / m.
v = v + dV.
calculate position change dS using v * t.
s = s + dS.
(Using vectors where appropriate, of course.)
Right now I'm doing one physics tick every frame, which is happening about 500-700 times per second. I'm aware that this will accumulate errors very quickly, but it should at least get me started.
(BTW, I was unable to find an off-the-shelf physics engine that handles orbital mechanics --- most 2D physics engines like Chipmunk and Box2D are more focused on rigid structures instead. Can anyone suggest one I could look at?)
You need to make sure that your delta t iterative time value is small enough. You will definitely have to tinker with the constants in order to get the behaviour you expect. Iterative simulation in your case and most cases is a form of integration where errors build up fast and unpredictably.
Yes, these equations hold in 2D space, because your 2D space is just a 2D representation of a 3D world. (A "real" 2D universe would have different equations, but that's not relevant here.)
A long shot: Are you perhaps using distance to the surface of the planet as r?
If that isn't it, try cutting your time step in half; if that makes a big difference, keep reducing it until the behavior stops changing.
If that makes no difference, try setting the initial velocity to zero, then watching it fall for a few iterations and measuring its acceleration to see if it's GM/r2. If the answer still isn't clear, post the results and we'll try to figure it out.

Trajectory -math, c#

I am trying to map a trajectory path between two point. All I know is the two points in question and the distance between them. What I would like to be able to calculate is the velocity and angle necessary to hit the end point.
I would also like to be able to factor in some gravity and wind so that the path/trajectory is a little less 'perfect.' Its for a computer game.
Thanks F.
This entire physical situation can be described using the SUVAT equations of motion, since the acceleration at all times is constant.
The following explanation presumes understanding of basic algebra and vector maths. If you're not familiar with it, I strongly recommend you go read up on it before attempting to write the sort of game you have proposed. It also assumes you're dealing with 2D, though if you're dealing with 3D most of the same applies, since it's all in vector form - you just end up solving a cubic instead of quadratic, for which it may be best to use a numerical solver.
Physics
(Note: vectors represented in bold.)
Basically, you'll want to start by formulating your equation for displacement (in vector form):
r = ut + (at^2)/2
r is the displacement relative to the start position, u is the initial velocity, a is the acceleration (constant at all times). t is of course time.
a is dependent on the forces present in your system. In the general case of gravity and wind:
a = F_w/m - g j
where i is the unit vector in the x direction and j the unit vector in the y direction. g is the acceleration due to gravity (9.81 ms^-2 on Earth). F_w is the force vector due to the wind (this term disappears for no wind) - we're assuming this is constant for the sake of simplicity. m is the mass of the projectile.
Then you can simply substitute the equation for a into the equation for r, and you're left with an equation of three variables (r, u, t). Next, expand your single vector equation for r into two scalar equations (for x and y displacement), and use substitution to eliminate t (maths might get a bit tricky here). You should be left with a single quadratic equation with only r and u as free variables.
Now, you want to solve the equation for r = [target position] - [start position]. If you pick a certain magnitude for the initial velocity u (i.e. speed), then you can write the x and y components of u as U cos(a) and U sin(a) respectively, where U is the initial speed, and a the initial angle. This can be rearranged and with a bit of trignometry, you can finally solve for the angle a, giving you the launch velocity!
Algorithm
Most of the above description should be worked out on paper first. Then, it's simply a matter of writing a function to solve the quadratic formula and apply some inverse trigonometric functions to get the result.
P.S. Sorry for all the maths/physics in this post, but it was unavoidable! The OP seemed to be asking more about the physical rather than computational side of this, anyway, so that's what I've provided. Hopefully this is still useful to both the OP and other people.
The book:
Modern Exterior Ballistics: The Launch and Flight Dynamics of Symmetric Projectiles ISBN-13: 978-0764307201
Is the modern authority on ballistics. For accuracy you'll need the corrections:
http://www.dexadine.com/mccoy.html
If you need something free and less authoritative, Dr. Mann's 1909 classic The bullet's flight from powder to target is available on books.google.com.
-kmarsh
PS Poor ballistics in games is a particular pet peeve of mine, especially the "shoots flat to infinity" ballistic model.
As people have mentioned, while figuring out the angles between the points is relatively easy, determining the way that wind and gravity will affect the shot is more difficult.
Wind and gravity are both accelrating forces, though they act somewhat differently.
Gravity is easier, since it has both a constant direction (down) and magnitude regardless of the object. (Assuming that you're not shooting things with ridiculously high velocities). To calculate how gravity will affect the velocity of your object, just take the time since you last updated the velocity of the object, multiply it by your gravitational factor, and add it to your current velocity vector.
As a simple example, let's think of an object that is moving with a velocity of (3, 4, 7) in the x, y, z directions, with z being parallel with the force of gravity. You decide that your gravity value is -.3 You are ready to calculate the new velocity. When you check, you discover that 10 time units have passed since your last calculation (whatever your time units are...perhaps ticks or something). You take your time units (10), multiply by your gravity (-.3), which gives you -3. You add that to your Z, and your new velocity is (3, 4, 4). That's it. (This has been very simplified, but that should get you started.)
Wind is a bit different, if you want to do it right. If you want to do it a simple and easy way, you can make it like gravity...a constant force in a particular direction. But a more realistic way is to have the force be dependent on your current velocity vector. Put simply: if you're moving exactly with the wind, it shouldn't impart any force onto you. In this case, you simply calculate the magnitude of the force as the difference between its direction and your own.
A simple example of this might be if you were moving at (3, 0, 0), and the wind was moving at (5, 0, 0), and we can give the wind a strength of .5. (You also have to multiply by the time elapsed...for the sake of this example, to keep it simple, we'll leave the time-elapsed factor at 1) You calculate the difference in the vectors and multiply by your time difference (1), and discover that the difference is (2, 0, 0). You then multiply that vector by the wind strength, .5, and you discover that your velocity change is (1, 0, 0). Add that to your previous velocity, and you get (4, 0, 0)...so the wind has sped the object up slightly. If you waited another single time unit, you would have a difference of (1, 0, 0), multiplied by your strength of .5, so your final velocity would then be (4.5, 0, 0). As you can see, the wind provides less force as you become closer to it in velocity.) This is kind of neat, but may be overly complex for game ballistics.
The angle is easy, atan2(pB.x-pA.x,pB.y-pA.y). The velocity vector should be (pB-pA)*speed. And to add gravity/wind (gravity is just wind with a negative y component) add the (scaled) wind vector to your velocity at every simulation tick (you're basically adding it as acceleration).
Just a link, sorry : http://www.gamedev.net/reference/articles/article694.asp.
There is a lot of papers about game physics at gamedev. Have a look.
(BTW : wind only add some velocity to an object. The hard part is gravity.)

Resources