Firstly, I have done much googling and checking other stackoverflow posts about this, but cannot get a working reply or a snippet of working code. Maths is not my strength.
I need to have a routine that takes a camera point (CX,CY,CZ) and rotate it about a lookat point (LX,LY,LZ) by three rotation angles (RX,RY,RZ). Using euler rotations leads to gimbal lock in some cases which I need to avoid. So I heard about using quaternions.
I found this to convert the rotations into a quaternion
http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
and this to convert from a quaternion back to euler XYZ rotations
http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
They seem to work fine, but I need to know how to use the quaternion to rotate the CX,CY,CZ around LX,LY,LZ and then return the new CX,CY,CZ without issues of gimbal lock.
There is so much out there about this, that I am sure a good explanation and snippet of code will help not only me but many others in the future.
So please help if you can. Many thanks.
The short answer, if your quaternion is Q and the new camera point is C':
C' = Q*(C-L)*Q^-1 + L
where the points are augmented with Cw=0 and multiplication and inverse are according to quaternion rules.
Specifically, let D = C - L. Then we let F = Q*D:
Fw = Qw*0 - Qx*Dx - Qy*Dy - Qz*Dz
Fx = Qw*Dx + Qx*0 + Qy*Dz - Qz*Dy
Fy = Qw*Dy - Qx*Dz + Qy*0 + Qz*Dx
Fz = Qw*Dz + Qx*Dy - Qy*Dx + Qz*0
Finally, we get C' = F*Q^-1 + L:
Cw' = 0
Cx' = Fw*Qx - Fx*Qw + Fy*Qz - Fz*Qy + Lx
Cy' = Fw*Qy - Fx*Qz - Fy*Qw + Fz*Qx + Ly
Cz' = Fw*Qz + Fx*Qy - Fy*Qx - Fz*Qw + Lz
However, be aware that if you're creating the quaternion from an Euler representation, you'll still end up with gimbal lock. The gimbal lock is a property of the Euler representation and the quaternion will just represent the same transformation. To get rid of gimbal lock, you'll need to avoid the Euler representation altogether, unless I misunderstand how you're using it.
Related
I am trying to do a basic version of space war(http://en.wikipedia.org/wiki/Spacewar_%28video_game%29) but I cannot figure out how to do the inertia part
that is my code :
I should let the ship accelerate or slow down based on where it faces
model is the ship
vx and vy are velocity of x and y direction
theta are rotate degree
20 is for make it move slow
vx=model.vx+(cos (degrees model.theta))/20,
vy=model.vy+(sin (degrees model.theta))/20
but it does not seem right
Can someone help me?
I am horrible in physics!
A very accurate and efficient integration is to compute: PosNext = 2 * PosCurrent - PosPrevious + Acceleration*Timestep^2
It is called Verlet integration scheme. For Velocities you just update by: VelocityNext = (PosNext-PosCurrent)/TimeStep.
You can use your sine and cosine with the acceleration constant. Euler forward is not very accurate, try to avoid it.
I am working on a wearable device using the Invensense 9D (Accel+Gyro+Compass) which computes a quaternion for real-time orientation, using the embedded Digital Motion Processor running a Kalman filter. Sadly -- the DMP code is not something that Invensense allows end-users access to.
The resulting quaternions in our application suffer from some drift. But we've been able to work within that by taking the differences between the Euler angles over short time-frames. But Euler angles are obviously less than desirable because of their inherent issues (gimbal lock, etc).
Where I'm struggling is computing the difference between two quaternions -- which 'should' be Diff = q2 * conj(q1) -- then computing the Pitch and Roll changes required to get from q1 to q2. That's where my brain freezes.
Appreciate any suggestions/pointers.
UPDATE :
I do indeed need to display the angular differences -- in both pitch and roll -- to a user so that they can understand the orientation difference between the two orientations.
FS.Roll <<- atan2((2.0*(FS.Quat.Q0*FS.Quat.Q1 + FS.Quat.Q2*FS.Quat.Q3)), (1.0 - 2.0*(FS.Quat.Q1^2 + FS.Quat.Q2^2))) * M_PI;
FS.Pitch <<- asin((2.0*(FS.Quat.Q0*FS.Quat.Q2 - FS.Quat.Q3*FS.Quat.Q1))) * M_PI;
MP.Roll <<- atan2((2.0*(MP.Quat.Q0*MP.Quat.Q1 + MP.Quat.Q2*MP.Quat.Q3)), (1.0 - 2.0*(MP.Quat.Q1^2 + MP.Quat.Q2^2))) * M_PI;
MP.Pitch <<- asin((2.0*(MP.Quat.Q0*MP.Quat.Q2 - MP.Quat.Q3*MP.Quat.Q1))) * M_PI;
If I take the differences between the FS.Pitch and MP.Pitch and the corresponding FS.Roll and MP.Roll -- I get exactly what I'm after.
But I'd like to reduce the inverse trig calcs on the embedded Cortex-M0 MCU -- as well as avoiding Gimbal Lock for each conversion -- so ideally I'd like to get the Quaternion 'Difference' between the two and decompose it into the relative Pitch and Roll components.
When I try your suggestion -- it yields different results than the Euler math.
I understand that there are number of different rotation sequences to get from a Quaternion to an Euler angle. I've tried over a dozen of them, none of which yields the same answer as taking the difference of the Euler angles from the individual Quaternions.
It feels like I'm missing something obvious -- but I'm just not seeing it.
Thanks!
For two quaternion orientations p and q the quaternion that would take p into the q frame is r = p* ⊗ q.
Then when you multiply:
p ⊗ r, quaternions are associative so:
p ⊗ r = p ⊗ (p* ⊗ q) = (p ⊗ p*) ⊗ q = q.
To get the Euler angles of r, you should use a library. Here's my Python code for it, if it helps:
def quat2eulerXYZ(q):
r = atan2(2*q[0][0]*q[1][0] - 2*q[2][0]*q[3][0], q[0][0]**2 - q[1][0]**2 - q[2][0]**2 + q[3][0]**2)
p = asin(2*q[0][0]*q[2][0] + 2*q[1][0]*q[3][0])
y = atan2(2*q[0][0]*q[3][0] - 2*q[1][0]*q[2][0], q[0][0]**2 + q[1][0]**2 - q[2][0]**2 - q[3][0]**2)
return (r, p, y)
If your AHRS system is drifting, it is probably the magnetometer suffering from interference which may be solved by calibrating for soft/hard iron interference. If your AHRS system is in close proximity or fastened to any ferrous metals, try it without them. If you try this outdoors, clear of ferrous metals, I would be surprised if it did not work.
You shouldn't need to convert to Euler angles once you have a quaternion, unless it is for display purposes. If you elaborate on what you are doing with the Euler angles I may be of more assistance.
After some time searching, I have revised my question.
I have found numerous examples of ball to ball collisions, but the only ones that seem to work use Vector2d or Vector2D.
This is a problem, because I am only allowed to use the regular java library, so my main question is: How do I convert the examples (which I will post below) to use what I can use?
I have several variables, both balls have the same mass, the velocities are broken into different variables, x and y. Also I have access to their x and y pos.
This is the ONLY problem left in my application.
I am at a total loss on how to convert the below example.
// get the mtd
Vector2d delta = (position.subtract(ball.position));
float d = delta.getLength();
// minimum translation distance to push balls apart after intersecting
Vector2d mtd = delta.multiply(((getRadius() + ball.getRadius())-d)/d);
// resolve intersection --
// inverse mass quantities
float im1 = 1 / getMass();
float im2 = 1 / ball.getMass();
// push-pull them apart based off their mass
position = position.add(mtd.multiply(im1 / (im1 + im2)));
ball.position = ball.position.subtract(mtd.multiply(im2 / (im1 + im2)));
// impact speed
Vector2d v = (this.velocity.subtract(ball.velocity));
float vn = v.dot(mtd.normalize());
// sphere intersecting but moving away from each other already
if (vn > 0.0f) return;
// collision impulse
float i = (-(1.0f + Constants.restitution) * vn) / (im1 + im2);
Vector2d impulse = mtd.multiply(i);
// change in momentum
this.velocity = this.velocity.add(impulse.multiply(im1));
ball.velocity = ball.velocity.subtract(impulse.multiply(im2));
Here is the URL for the question:
http://stackoverflow.com/questions/345838/ball-to-ball-collision-detection-and-handling
And I have taken a look at his source code.
Thank you for taking the time to read this issue.
SUCCESS!
I have found how to use Vector2d, and it works PERFECTLY!
Will edit later with answer!
I'm implementing my own 3d engine in c# based on a really basic 3d open-source engine in JavaScript called a3. I don't know If I have 100% understand you but It sounds like you can only find examples with Vector2d but you are not allowed to use that class?
I that is the case, as you can imagine javascript does not have native Vector2d types so someone had to implement. Don't be afraid of giving it a try, is just a few high school maths functions, you should be able to implement your own Vector2d class in just a few minutes
The following link contain implementations if vector2d, vector3d, vector4d, matrix3, and matrix4 in javascript: https://github.com/paullewis/a3/tree/master/src/js/core/math hope it helps :)
I am trying to calculate the forces that will act on circular objects in the event of a collision. Unfortunately, my mechanics is slightly rusty so i'm having a bit of trouble.
I have an agent class with members
vector position // (x,y)
vector velocity // (x,y)
vector forward // (x,y)
float radius // radius of the agent (all circles)
float mass
So if we have A,B:Agent, and in the next time step the velocity is going to change the position. If a collision is going to occur I want to work out the force that will act on the objects.
I know Line1 = (B.position-A.position) is needed to work out the angle of the resultant force but how to calculate it is baffling me when I have to take into account current velocity of the vehicle along with the angle of collision.
arctan(L1.y,L1.x) is am angle for the force (direction can be determined)
sin/cos are height/width of the components
Also I know to calculate the rotated axis I need to use
x = cos(T)*vel.x + sin(T)*vel.y
y = cos(T)*vel.y + sin(T)*vel.x
This is where my brain can't cope anymore.. Any help would be appreciated.
As I say, the aim is to work out the vector force applied to the objects as I have already taken into account basic physics.
Added a little psudocode to show where I was starting to go with it..
A,B:Agent
Agent {
vector position, velocity, front;
float radius,mass;
}
vector dist = B.position - A.position;
float distMag = dist.magnitude();
if (distMag < A.radius + B.radius) { // collision
float theta = arctan(dist.y,dist.x);
flost sine = sin(theta);
float cosine = cos(theta);
vector newAxis = new vector;
newAxis.x = cosine * dist .x + sine * dist .y;
newAxis.y = cosine * dist .y - sine * dist .x;
// Converted velocities
vector[] vTemp = {
new vector(), new vector() };
vTemp[0].x = cosine * agent.velocity.x + sine * agent.velocity.y;
vTemp[0].y = cosine * agent.velocity.y - sine * agent.velocity.x;
vTemp[1].x = cosine * current.velocity.x + sine * current.velocity.y;
vTemp[1].y = cosine * current.velocity.y - sine * current.velocity.x;
Here's to hoping there's a curious maths geek on stack..
Let us assume, without loss of generality, that we are in the second object's reference frame before the collision.
Conservation of momentum:
m1*vx1 = m1*vx1' + m2*vx2'
m1*vy1 = m1*vy1' + m2*vy2'
Solving for vx1', vy1':
vx1' = vx1 - (m2/m1)*vx2'
vy1' = vy1 - (m2/m1)*vy2'
Secretly, I will remember the fact that vx1'*vx1' + vy1'*vy1' = v1'*v1'.
Conservation of energy (one of the things elastic collisions give us is that angle of incidence is angle of reflection):
m1*v1*v1 = m1*v1'*v1' + m2*v2'+v2'
Solving for v1' squared:
v1'*v1' = v1*v1 - (m2/m1)v2'*v2'
Combine to eliminate v1':
(1-m2/m1)*v2'*v2' = 2*(vx2'*vx1+vy2'*vy1)
Now, if you've ever seen a stationary poolball hit, you know that it flies off in the direction of the contact normal (this is the same as your theta).
v2x' = v2'cos(theta)
v2y' = v2'sin(theta)
Therefore:
v2' = 2/(1-m2/m1)*(vx1*sin(theta)+vy1*cos(theta))
Now you can solve for v1' (either use v1'=sqrt(v1*v1-(m2/m1)*v2'*v2') or solve the whole thing in terms of the input variables).
Let's call phi = arctan(vy1/vx1). The angle of incidence relative to the tangent line to the circle at the point of intersection is 90-phi-theta (pi/2-phi-theta if you prefer). Add that again for the reflection, then convert back to an angle relative to the horizontal. Let's call the angle of incidence psi = 180-phi-2*theta (pi-phi-2*theta). Or,
psi = (180 or pi) - (arctan(vy1/vx1))-2*(arctan(dy/dx))
So:
vx1' = v1'sin(psi)
vy1' = v1'cos(psi)
Consider: if these circles are supposed to be solid 3D spheres, then use a mass proportional to radius-cubed for each one (note that the proportionality constant cancels out). If they are supposed to be disklike, use mass proportional to radius-squared. If they are rings, just use radius.
Next point to consider: Since the computer updates at discrete time events, you actually have overlapping objects. You should back out the objects so that they don't overlap before computing the new location of each object. For extra credit, figure out the time that they should have intersected, then move them in the new direction for that amount of time. Note that this time is just the overlap / old velocity. The reason that this is important is that you might imagine a collision that is computed that causes the objects to still overlap (causing them to collide again).
Next point to consider: to translate the original problem into this problem, just subtract object 2's velocity from object 1 (component-wise). After the computation, remember to add it back.
Final point to consider: I probably made an algebra error somewhere along the line. You should seriously consider checking my work.
I wanted to simulate the following through animation :
A ball starts with a certain velocity at the bottom most point of
a vertical circular loop and keeps rolling in it until its velocity permits.
For this, I wanted to find velocity/x/y vs. time equation.
For e.g. if the ball had mass : 5Kg, radius of the circular loop = 10m,
and initial velocity of the ball is 200 m/s, what will its velocity and (x,y) position
be after 5 seconds?
thanks.
Sliding, frictionless case with a point-particle ball
In this case we aren't worrying about rotational energy and are assuming that the ball is actually a point particle. Then, in order for the ball to stay on at the top, the centripetal force condition has to be satisfied:
m * v_top^2 / r = m * g
so
v_top = sqrt(r * g)
So the minimum initial velocity is determined by:
1 / 2 * m * v0^2 >= 1 / 2 * m * v_top^2 + m * g * 2 * r
v0 >= sqrt(5 * r * g)
This is similar to what Pete said, except that he forgot the centripetal force condition to stay on at the top.
Next, the acceleration tangential to the track is given by:
a = - g * sin(theta)
but a = r * alpha = r * d^2(theta)/dt^2 where alpha is the rotational acceleration. Thus, we get
r * d^2(theta)/dt^2 = g * sin(theta)
However, I don't know of an analytical solution to this differential equation and Mathematica was stumbling with finding one too. You can't just move the dts to the other side and integrate because theta is a function of t. I would recommend solving it by numerical means such as a Runga-Kutte or maybe the Verlet method. I solved it using Mathematica for the parameters you gave, but with the ball moving so quickly, it doesn't really slow down much in going around. When I lowered the initial velocity though, I was able to see the speeding up and slowing down by plotting theta as a function of time.
Adding in other things like a finite ball radius, rotational energy and friction are certainly doable, but I would worry about being able to solve this first case before moving on because it only gets more complicated from here. By the way, with the friction you will have to choose some kinetic coefficient of friction for your given materials which will of course be proportional to the normal force exerted on the ball by the track which can be solved for by summing the force components along the radius of the circle and don't forget to include the centripetal force condition.
If you haven't done this sort of physics before, I definitely recommend getting a introductory good book on physics (with calculus) and working through it. You only need to bother with the sections that apply to mechanics though that is a very large section of the book probably. There might be better routes to pursue though like some of the resources in this question.
If there are no acceleration (x,y) =(xstart+ vx*time ,ystart + vy*time) and speed remain the same, and it is not related to the radius
Since the velocity is constant you will have an angular velocity of omega = vel / radius. You will obtain how many radians you ball will move per second over its circular path.
To get the position at time t you just have to exploit polar coordinates:
x = x_center + sin( 3/2*PI + omega*t)*radius
y = y_center + cos( 3/2*PI + omega*t)*radius
This because you start from bottom point of the circle (so its 3/2*PI) plus how many radiants you move every second (we obtained it from tangential velocity). All multiplied for the radius, otherwise you will consider a unity circle.
EDIT: Since you wonder how to find a position of an object that is subject to many different forces I can tell you that usually a physical engine doesn't care about finding equations of moving objects. It just applies forces to objects considering their intended motions (like your circular one) or environmental factors (like gravity or friction) and calculates coordinates step by step by applying forces and using an integrator to see the results.
Ignoring friction, the forces on the ball are gravity and the track.
First, there are two main cases - is the velocity enough for the ball to loop-the-loop or not:
initial energy = 1/2 m v² = 0.5 * 5 * 200 * 200
potential energy = m g h = 5 * 9.8 * 20
so it will go round the whole loop.
Initially the ball is at the bottom of the loop, theta = 0
The acceleration on the ball is the component of g along the track
a = g⋅sin theta
The distance travelled is theta * radius. It is also the double integral of acceleration against time.
theta ⋅ radius = double integral of acceleration against time
Integrating acceleration once gives velocity, integrating velocity gives distance.
so solve this for t:
theta ⋅ r = ∫(∫ g⋅sin theta.dt).dt
then your x and y are trivial functions of theta.
Whether you solve it analytically or numerically is up to you.
With dynamic friction, friction is usually proportional to the normal force on the bodies. So this will equal the centripetal force - proportional to the square of the angular velocity, and the component of gravity normal to the track (g sin theta)
You didn't tell anything about how you want your velocity to change. Do you have any friction model? If there is no friction, then the formulas are simple:
length = velocity*t
x = sin(length)*radius
y = -cos(length)*radius
If the velocity is changing, then you have to change length to something like
length = integral over dt[0..t] (velocity dt)
The only thing I wanted to add is the if this is real ball (sphere) with mass 5kg then it must have a diameter dia=(6*m/(PI*rho))^(1/3) where rho is the density of the material. For steel (rho=7680) the diameter is dia=0.1075 meters. Therefore the pitch radius (radius at which the center of gravity of the ball rides on) is equal to R=10-(dia/2) or R=9.9466 meters.
The problem gets a little more complex when friction is included. For one you have to consider the direction of friction (assuming dry friction theory). That depends on the amount the ball rotates in its axis and that depends on moment of inertia of the ball.
When you do the simulation you might want to monitor the total kinetic energy + the total potential energy and make sure your are not adding energy to the system (or taking away). [Don't forget to include the rotational component for the kinetic energy]
Get a standard book on dynamics, and I am sure a similar problem is already described in the book.I would recommend "Vector Mechanic for Engineers - Dynamics".