I have a rotation quaternion and want to extract the angle of rotation about the Up axis (the yaw). I am using XNA and as far as I can tell there is no inbuilt function for this. What is the best way to do this?
Thanks for any help,
Venatu
The quaternion representation of rotation is a variation on axis and angle. So if you rotate by r radians around axis x, y, z, then your quaternion q is:
q[0] = cos(r/2);
q[1] = sin(r/2)*x;
q[2] = sin(r/2)*y;
q[3] = sin(r/2)*z;
If you want to create a quaternion that only rotates around the y axis, you zero out the x and z axes and then re-normalize the quaternion:
q[1] = 0;
q[3] = 0;
double mag = sqrt(q[0]*q[0] + q[2]*q[2]);
q[0] /= mag;
q[2] /= mag;
If you want the resulting angle:
double ang = 2*acos(q[0]);
This assumes that the quaternion representation is stored: w,x,y,z. If both q[0] and q[2] are zero, or close to it, the resulting quaternion should just be {1,0,0,0}.
Having given a Quaternion q, you can calculate roll, pitch and yaw like this:
var yaw = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
var pitch = asin(-2.0*(q.x*q.z - q.w*q.y));
var roll = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
This should fit for intrinsic tait-bryan rotation of xyz-order. For other rotation orders, extrinsic and proper-euler rotations other conversions have to be used.
Note: I've verified below code against Wikipedia's equations plus Pixhawk's documentation and it is correct.
If you are working with drones/aviation, below is the code (taken directly from DJI SDK). Here q0, q1, q2, q3 corresponds to w,x,y,z components of the quaternion respectively. Also note that yaw, pitch, roll may be referred to as heading, attitude and bank respectively in some literature.
float roll = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
float yaw = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));
If you need to calculate all 3 then you can avoid recalculating common terms by using following functions:
//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152
EulerianAngle Flight::toEulerianAngle(QuaternionData data)
{
EulerianAngle ans;
double q2sqr = data.q2 * data.q2;
double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0;
double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3);
double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2);
double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1);
double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0;
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
ans.pitch = asin(t2);
ans.roll = atan2(t3, t4);
ans.yaw = atan2(t1, t0);
return ans;
}
QuaternionData Flight::toQuaternion(EulerianAngle data)
{
QuaternionData ans;
double t0 = cos(data.yaw * 0.5);
double t1 = sin(data.yaw * 0.5);
double t2 = cos(data.roll * 0.5);
double t3 = sin(data.roll * 0.5);
double t4 = cos(data.pitch * 0.5);
double t5 = sin(data.pitch * 0.5);
ans.q0 = t2 * t4 * t0 + t3 * t5 * t1;
ans.q1 = t3 * t4 * t0 - t2 * t5 * t1;
ans.q2 = t2 * t5 * t0 + t3 * t4 * t1;
ans.q3 = t2 * t4 * t1 - t3 * t5 * t0;
return ans;
}
Note on Eigen Library
If you are using Eigen library, it has another way to do this conversion, however, this may not be as optimized as above direct code:
Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
yaw = euler[0]; pitch = euler[1]; roll = euler[2];
Conversion Quaternion to Euler
I hope you know that yaw, pitch and roll are not good for arbitrary rotations. Euler angles suffer from singularities (see the above link) and instability. Look at 38:25 of the presentation of David Sachs
http://www.youtube.com/watch?v=C7JQ7Rpwn2k
Good luck!
A quaternion consists of two components: a 3d vector component and a scalar component.
The vector component of the quaternion describes independent rotations about each axis, so zero'ing out the x- and y-components of the vector component and leaving z-component as-is is all you need to do in order to solve for the vector term:
// Don't modify qz
double qx = 0;
double qy = 0;
The scalar term represents the magnitude of rotation. For a unit quaternion (such as one used to represent attitude), the entire quaternion must have a magnitude of 1. Thus, the scalar term can be solved by:
double qw = sqrt(1 - qx*qx - qy*qy - qz*qz);
Since qx and qy are zero, the scalar component is given by
double qw = sqrt(1 - qz*qz);
Thus, the full quaternion representing yaw is given by
double qx = 0;
double qy = 0;
// Don't modify qz
double qw = sqrt(1 - qz*qz);
The transformation from quaternion to yaw, pitch, and roll depends on the conventions used to define the quaternion and the yaw, pitch, and roll. For a given convention there are many "almost correct" transformations that will work for the majority of angles but only one truly correct transformation that will work for all angles including south and north poles where the "almost correct" transformations produce gimbal locks (spurious flips and rotations).
See this tutorial for more information:
https://youtu.be/k5i-vE5rZR0
Related
I come accross a math problem about Interactive Computer Graphics.
I summarize and abstract this problem as follows:
I'm going to rotation a 3d coordinate P(x1,y1,z1) around a point O(x0,y0,z0)
and there are 2 vectors u and v which we already know.
u is the direction to O before transformation.
v is the direction to O after transformation.
I want to know how to conduct the calculation and get the coordinate of Q
Thanks a lot.
Solution:
Rotation About an Arbitrary Axis in 3 Dimensions using the following matrix:
rotation axis vector (normalized): (u,v,w)
position coordinate of the rotation center: (a,b,c)
rotation angel: theta
Reference:
https://docs.google.com/viewer?a=v&pid=sites&srcid=ZGVmYXVsdGRvbWFpbnxnbGVubm11cnJheXxneDoyMTJiZTZlNzVlMjFiZTFi
for just single point no rotations is needed ... so knowns are:
u,v,O,P
so we now the distance is not changing:
|P-O| = |Q-O|
and directions are parallel to u,v so:
Q = O + v*(|P-O|/|v|)
But I suspect you want to construct rotation (transform matrix) such that more points (mesh perhaps) are transformed. If that is true then you need at least one known to get this right. Because there is infinite possible rotations transforming P -> Q but the rest of the mesh will be different for each ... so you need to know at least 2 non trivial points pair P0,P1 -> Q0,Q1 or axis of rotation or plane parallel to rotation or any other data known ...
Anyway in current state you can use as rotation axis vector perpendicular to u,v and angle obtained from dot product:
axis = cross (u,v)
ang = +/-acos(dot(u,v))
You just need to find out the sign of angle so try both and use the one for which the resultinq Q is where it should be so dot(Q-O,v) is max. To rotate around arbitrary axis and point use:
Rodrigues_rotation_formula
Also this might be helpfull:
Understanding 4x4 homogenous transform matrices
By computing dot product between v and u get the angle l between the vectors. Do a cross product of v and u (normalized) to produce axis of rotation vector a. Let w be a vector along vector u from O to P. To rotate point P into Q apply the following actions (in pseudo code) having axis a and angle l computed above:
float4 Rotate(float4 w, float l, float4 a)
{
float4x4 Mr = IDENTITY;
quat_t quat = IDENTITY;
float4 t = ZERO;
float xx, yy, zz, xy, xz, yz, wx, wy, wz;
quat[X] = a[X] * sin((-l / 2.0f));
quat[Y] = a[Y] * sin((-l / 2.0f));
quat[Z] = a[Z] * sin((-l / 2.0f));
quat[W] = cos((-l / 2.0f));
xx = quat[X] * quat[X];
yy = quat[Y] * quat[Y];
zz = quat[Z] * quat[Z];
xy = quat[X] * quat[Y];
xz = quat[X] * quat[Z];
yz = quat[Y] * quat[Z];
wx = quat[W] * quat[X];
wy = quat[W] * quat[Y];
wz = quat[W] * quat[Z];
Mr[0][0] = 1.0f - 2.0f * (yy + zz);
Mr[0][1] = 2.0f * (xy + wz);
Mr[0][2] = 2.0f * (xz - wy);
Mr[0][3] = 0.0f;
Mr[1][0] = 2.0f * (xy - wz);
Mr[1][1] = 1.0f - 2.0f * (xx + zz);
Mr[1][2] = 2.0f * (yz + wx);
Mr[1][3] = 0.0f;
Mr[2][0] = 2.0f * (xz + wy);
Mr[2][1] = 2.0f * (yz - wx);
Mr[2][2] = 1.0f - 2.0f * (xx + yy);
Mr[2][3] = 0.0f;
Mr[3][0] = 0.0f;
Mr[3][1] = 0.0f;
Mr[3][2] = 0.0f;
Mr[3][3] = 1.0f;
w = Mr * w;
return w;
}
Point Q is at the end of the rotated vector w. Algorithm used in the pseudo code is quaternion rotation.
If you know u, v, P, and O then I would suggest that you compute |OP| which should be preserved under rotations. Then multiply this length by the unit vector -v (I assumed u, v are unit vectors: if not - normalize them) and translate the origin by this -|OP|v vector. The negative sign in front of v comes from the description given in your question:"v is the direction to O after transformation".
P and Q are at the same distance R to O
R = sqrt( (x1-x0)^2 + (y1-y0)^2 + (z1-z0)^2 )
and OQ is collinear to v, so OQ = v * R / ||v|| where ||v|| is the norm of v
||v|| = sqrt( xv^2 + yv^2 + zv^2 )
So the coordinates of Q(xq,yq,zq) are:
xq= xo + xv * R / ||v||
yq= yo + yv * R / ||v||
zq= zo + zv * R / ||v||
I have inertial measurement unit sensors that can output data in either quaternions or Euler angles. As a biomechanist, Euler angles make more sense to me, but I sorta understand quaternions as well, but I've never really studied them. I do have a math background, so I'm not completely lost and I understand the Gimbal lock effect in Euler Angles.
I'm looking to calculate angles between two vectors whether they're quaternions or Euler angles, specifically on a human subject. I basically want to find the axis of rotations and calculate the difference in angles in the three basic components (x,y,z), and it seems incredibly unlikely that a person would be able to contort their body and reach Gimbal-lock.
I've read this paper and it seems like the way you choose to approach a rotation (x->y->z gets you to the same point as x->z->y but are different paths in terms of angles taken) is where Gimbal-lock comes into play, but the proposed XZ'Y'' sequence seems to avoid Gimbal-lock altogether.
I've read that quaternions are simply easier for computers to calculate which is where I would like to keep using quaternions since I am using a Pi, but I just don't quite fully understand how to go from quaternions to your basic x,y,z components. So I guess my questions are:
Are quaternions necessary for human movement?
Would maintaining numbers in quaternions until the very final angle calculations and converting to Euler angle in the last step avoid Gimbal lock?
Basically, you have 2 main options to work e.g. with a skeleton.
Use 4x4 matrix (which allows rotation and translation)
Use (unit) quaternions for rotation and offsets for translation.
If you look at a typical implementation of a function taking 2 vectors and returning a quaternion, giving the rotation between them, you will see that it is not just a simple formula. Edge cases are being identified and taken care of.
let rotFromVectors (v1 : vec3) (v2 : vec3) : quat =
let PI = System.Math.PI
let PI_BY_TWO = PI / 2.0
let TWO_PI = 2.0 * PI
let ZERO_ROTATION = quat(0.0f,0.0f,0.0f,1.0f)
let aabb = sqrt (float (vec3.dot(v1, v1)) * float (vec3.dot(v2,v2)))
if aabb <> 0.0
then
let ab = float (vec3.dot(v1,v2)) / aabb
let c =
vec3
( float32 ((float v1.y * float v2.z - float v1.z * float v2.y) / aabb)
, float32 ((float v1.z * float v2.x - float v1.x * float v2.z) / aabb)
, float32 ((float v1.x * float v2.y - float v1.y * float v2.x) / aabb)
)
let cc = float (vec3.dot(c, c))
if cc <> 0.0
then
let s =
match ab > -sin (PI_BY_TWO) with //0.707107f
| true -> 1.0 + ab
| false -> cc / (1.0 + sqrt (1.0-cc))
let m = sqrt (cc + s * s)
quat(float32 (float c.x / m), float32 (float c.y / m), float32 (float c.z / m), float32(s / m))
else
if ab > 0.0
then
ZERO_ROTATION
else
let m = sqrt (v1.x * v1.x + v1.y * v1.y)
if(m <> 0.0f)
then
quat(v1.y / m, (-v1.x) / m, 0.0f, 0.0f)
else
quat(1.0f,0.0f,0.0f,0.0f)
else
ZERO_ROTATION
Where quat is the type for a quaternion and vec3 the type of a 3D vector in the code above.
The code to rotate a vector by a quaternion is just about as straightforward as the math suggests:
let rotateVector (alpha : quat) (v:vec3) : vec3 =
let s = vec3.length v
quat.inverse alpha * (vecToPureQuat v) * alpha |> pureQuatToVec |> fun v' -> v' * s
And last not least the conversion functions between (some... Euler angles - there are actually 24 different versions of Euler angles, 12 with fixed angle rotations and 12 with consecutive rotations) uses the half angle approach.
let eulerToRot (v:vec3) : quat =
let d = 0.5F
let t0 = cos (v.z * d)
let t1 = sin (v.z * d)
let t2 = cos (v.y * d)
let t3 = sin (v.y * d)
let t4 = cos (v.x * d)
let t5 = sin (v.x * d)
quat
( t0 * t3 * t4 - t1 * t2 * t5
, t0 * t2 * t5 + t1 * t3 * t4
, t1 * t2 * t4 - t0 * t3 * t5
, t0 * t2 * t4 + t1 * t3 * t5
)
|> quat.normalize
let rotToEuler (q:quat) : vec3 =
let ysqr = q.y * q.y
// roll (x-axis rotation)
let t0 = +2.0f * (q.w * q.x + q.y * q.z)
let t1 = +1.0f - 2.0f * (q.x * q.x + ysqr)
let roll = atan2 t0 t1
// pitch (y-axis rotation)
let t2 =
let t2' = +2.0f * (q.w * q.y - q.z * q.x)
match t2' with
| _ when t2' > 1.0f -> 1.0f
| _ when t2' < -1.0f -> -1.0f
| _ -> t2'
let pitch = asin t2
// yaw (z-axis rotation)
let t3 = +2.0f * (q.w * q.z + q.x *q.y)
let t4 = +1.0f - 2.0f * (ysqr + q.z * q.z)
let yaw = atan2 t3 t4
vec3(roll,pitch,yaw)
The final trick to know is, that to turn a vector into a (pure) quaternion comes in handy for the rotateVector function.
let vecToPureQuat (v:vec3) : quat =
quat(v.x,v.y,v.z,0.0f)
let pureQuatToVec (q:quat) : vec3 =
vec3(q.x,q.y,q.z)
So, to answer your main question: Are quaternions necessary? No. You can as well use 4x4 matrices.
And you can go from one to the other if it deems you useful:
let offsetAndRotToMat (offset:vec3) (q:quat) : mat4 =
let ux = v3 1 0 0
let uy = v3 0 1 0
let uz = v3 0 0 1
let rx = rotateVector q ux
let ry = rotateVector q uy
let rz = rotateVector q uz
mat4
(
rx.x, rx.y, rx.z, 0.0f,
ry.x, ry.y, ry.z, 0.0f,
rz.x, rz.y, rz.z, 0.0f,
offset.x,offset.y,offset.z,1.0f
)
I'm having trouble understanding the math behind this function. I would like to hear the logic behind the formulas (especially what is this tangential and radial factor) written here to create points which later (when it send the vec3 array to a function) form a circle in OpenGL.
void doTesselate(const Arc& arc, int slices, std::vector<glm::vec3>& vertices)
{
double dang = (arc.endAngle() - arc.startAngle()) * Deg2Rad;
double radius = arc.radius();
double angIncr = dang / slices;
double tangetial_factor = tan(angIncr);
double radial_factor = 1 - cos(angIncr);
double startAngle = arc.startAngle() * Deg2Rad;
const glm::vec3& center = arc.center();
double x = center.x - radius * cos(startAngle);
double y = center.y - radius * sin(startAngle);
++slices;
for (int ii = 0; ii < slices; ii++) {
vertices.push_back(glm::vec3(x, y, center.z));
double tx = center.y - y;
double ty = x - center.x;
x += tx * tangetial_factor;
y += ty * tangetial_factor;
double rx = center.x - x;
double ry = center.y - y;
x += rx * radial_factor;
y += ry * radial_factor;
}
}
The idea is the following:
Starting from the current point, you go a bit in tangential direction and then back towards the center.
The vector (tx, ty) is the tangent at the current point with length equal to the radius. In order to get to the new angle, you have to move tan(angle) * radius along the tangent. radius is already incorporated in the tangent vector and tan(angle) is the tangetial_factor (you get that directly from tangent's definition).
After that, (rx, ry) is the vector towards the center. This vector has the length l:
cos(angle) = radius / l
l = radius / cos(angle)
We need to find a multiple m of this vector, such that the corrected point lies on the circle with the given radius again. If we just inspect the lengths, then we want to find:
target distance = current distance - m * length of (rx, ry)
radius = radius / cos(angle) - m * radius / cos(angle)
1 = (1 - m) / cos(angle)
cos(angle) = 1 - m
1 - cos(angle) = m
And this multiple is exactly the radial_factor (the amount which you need to move towards the center to get onto the circle).
Assuming I have a spaceship (source); And an asteroid (target) is somewhere near it.
I know, in 3D space (XYZ vectors):
My ship's position (sourcePos) and velocity (sourceVel).
The asteroid's position (targetPos) and velocity (targetVel).
(eg. sourcePos = [30, 20, 10]; sourceVel = [30, 20, 10]; targetPos = [600, 400, 200]; targetVel = [300, 200, 100]`)
I also know that:
The ship's velocity is constant.
The asteroid's velocity is constant.
My ship's projectile speed (projSpd) is constant.
My ship's projectile trajectory, after being shot, is linear (/straight).
(eg. projSpd = 2000.00)
How can I calculate the interception coordinates I need to shoot at in order to hit the asteroid?
Notes:
This question is based on this Yahoo - Answers page.
I also searched for similar problems on Google and here on SO, but most of the answers are for 2D-space, and, of the few for 3D, neither the explanation nor the pseudo-codes explain what is doing what and/or why, so I couldn't really understand enough to apply them on my code successfully. Here are some of the pages I visited:
Danik Games Devlog, Blitz3D Forums thread, UnityAnswers, StackOverflow #1, StackOverflow #2
I really can't figure out the maths / execution-flow on the linked pages as they are, unless someone dissects it (further) into what is doing what, and why;
Provides a properly-commented pseudo-code for me to follow;
Or at least points me to links that actually explain how the equations work instead of just throwing even more random numbers and unfollowable equations in my already-confused psyche.
I find the easiest approach to these kind of problems to make sense of them first, and have a basic high school level of maths will help too.
Solving this problem is essentially solving 2 equations with 2 variables which are unknown to you:
The vector you want to find for your projectile (V)
The time of impact (t)
The variables you know are:
The target's position (P0)
The target's vector (V0)
The target's speed (s0)
The projectile's origin (P1)
The projectile's speed (s1)
Okay, so the 1st equation is basic. The impact point is the same for both the target and the projectile. It is equal to the starting point of both objects + a certain length along the line of both their vectors. This length is denoted by their respective speeds, and the time of impact. Here's the equation:
P0 + (t * s0 * V0) = P1 + (t * s0 * V)
Notice that there are two missing variables here - V & t, and so we won't be able to solve this equation right now. On to the 2nd equation.
The 2nd equation is also quite intuitive. The point of impact's distance from the origin of the projectile is equal to the speed of the projectile multiplied by the time passed:
We'll take a mathematical expression of the point of impact from the 1st equation:
P0 + (t * s0 * V0) <-- point of impact
The point of origin is P1
The distance between these two must be equal to the speed of the projectile multiplied by the time passed (distance = speed * time).
The formula for distance is: (x0 - x1)^2 + (y0 - y1)^2 = distance^2, and so the equation will look like this:
((P0.x + s0 * t * V0.x) - P1.x)^2 + ((P0.y + s0 * t * V0.y) - P1.y)^2 = (s1 * t)^2
(You can easily expand this for 3 dimensions)
Notice that here, you have an equation with only ONE unknown variable: t!. We can discover here what t is, then place it in the previous equation and find the vector V.
Let me solve you some pain by opening up this formula for you (if you really want to, you can do this yourself).
a = (V0.x * V0.x) + (V0.y * V0.y) - (s1 * s1)
b = 2 * ((P0.x * V0.x) + (P0.y * V0.y) - (P1.x * V0.x) - (P1.y * V0.y))
c = (P0.x * P0.x) + (P0.y * P0.y) + (P1.x * P1.x) + (P1.y * P1.y) - (2 * P1.x * P0.x) - (2 * P1.y * P0.y)
t1 = (-b + sqrt((b * b) - (4 * a * c))) / (2 * a)
t2 = (-b - sqrt((b * b) - (4 * a * c))) / (2 * a)
Now, notice - we will get 2 values for t here.
One or both may be negative or an invalid number. Obviously, since t denotes time, and time can't be invalid or negative, you'll need to discard these values of t.
It could very well be that both t's are bad (in which case, the projectile cannot hit the target since it's faster and out of range). It could also be that both t's are valid and positive, in which case you'll want to choose the smaller of the two (since it's preferable to hit the target sooner rather than later).
t = smallestWhichIsntNegativeOrNan(t1, t2)
Now that we've found the time of impact, let's find out what the direction the projectile should fly is. Back to our 1st equation:
P0 + (t * s0 * V0) = P1 + (t * s0 * V)
Now, t is no longer a missing variable, so we can solve this quite easily. Just tidy up the equation to isolate V:
V = (P0 - P1 + (t * s0 * V0)) / (t * s1)
V.x = (P0.x - P1.x + (t * s0 * V0.x)) / (t * s1)
V.y = (P0.y - P1.y + (t * s0 * V0.y)) / (t * s1)
And that's it, you're done!
Assign the vector V to the projectile and it will go to where the target will be rather than where it is now.
I really like this problem since it takes math equations we learnt in high school where everyone said "why are learning this?? we'll never use it in our lives!!", and gives them a pretty awesome and practical application.
I hope this helps you, or anyone else who's trying to solve this.
If you want a projectile to hit asteroid, it should be shoot at the point interceptionPos that satisfy the equation:
|interceptionPos - sourcePos| / |interceptionPos - targetPos| = projSpd / targetVel
where |x| is a length of vector x.
In other words, it would take equal amount of time for the target and the projectile to reach this point.
This problem would be solved by means of geometry and trigonometry, so let's draw it.
A will be asteroid position, S - ship, I - interception point.
Here we have:
AI = targetVel * t
SI = projSpd * t
AS = |targetPos - sourcePos|
vector AS and AI direction is defined, so you can easily calculate cosine of the SAI angle by means of simple vector math (take definitions from here and here). Then you should use the Law of cosines with the SAI angle. It will yield a quadratic equation with variable t that is easy to solve (no solutions = your projectile is slower than asteroid). Just pick the positive solution t, your point-to-shoot will be
targetPos + t * targetVel
I hope you can write a code to solve it by yourself. If you cannot get something please ask in comments.
I got a solution. Notice that the ship position, and the asteroid line (position and velocity) define a 3D plane where the intercept point lies. In my notation below | [x,y,z] | denotes the magnitude of the vector or Sqrt(x^2+y^2+z^2).
Notice that if the asteroid travels with targetSpd = |[300,200,100]| = 374.17 then to reach the intercept point (still unknown, called hitPos) will require time equal to t = |hitPos-targetPos|/targetSpd. This is the same time the projectile needs to reach the intercept point, or t = |hitPos - sourcePos|/projSpd. The two equations are used to solve for the time to intercept
t = |targetPos-sourcePos|/(projSpd - targetSpd)
= |[600,400,200]-[30,20,10]|/(2000 - |[300,200,100]|)
= 710.81 / ( 2000-374.17 ) = 0.4372
Now the location of the intetception point is found by
hitPos = targetPos + targetVel * t
= [600,400,200] + [300,200,100] * 0.4372
= [731.18, 487.45, 243.73 ]
Now that I know the hit position, I can calculate the direction of the projectile as
projDir = (hitPos-sourcePos)/|hitPos-sourcePos|
= [701.17, 467.45, 233.73]/874.52 = [0.8018, 0.5345, 0.2673]
Together the projDir and projSpd define the projectile velocity vector.
Credit to Gil Moshayof's answer, as it really was what I worked off of to build this. But they did two dimensions, and I did three, so I'll share my Unity code in case it helps anyone along. A little long winded and redundant. It helps me to read it and know what's going on.
Vector3 CalculateIntercept(Vector3 targetLocation, Vector3 targetVelocity, Vector3 interceptorLocation, float interceptorSpeed)
{
Vector3 A = targetLocation;
float Ax = targetLocation.x;
float Ay = targetLocation.y;
float Az = targetLocation.z;
float As = targetVelocity.magnitude;
Vector3 Av = Vector3.Normalize(targetVelocity);
float Avx = Av.x;
float Avy = Av.y;
float Avz = Av.z;
Vector3 B = interceptorLocation;
float Bx = interceptorLocation.x;
float By = interceptorLocation.y;
float Bz = interceptorLocation.z;
float Bs = interceptorSpeed;
float t = 0;
float a = (
Mathf.Pow(As, 2) * Mathf.Pow(Avx, 2) +
Mathf.Pow(As, 2) * Mathf.Pow(Avy, 2) +
Mathf.Pow(As, 2) * Mathf.Pow(Avz, 2) -
Mathf.Pow(Bs, 2)
);
if (a == 0)
{
Debug.Log("Quadratic formula not applicable");
return targetLocation;
}
float b = (
As * Avx * Ax +
As * Avy * Ay +
As * Avz * Az +
As * Avx * Bx +
As * Avy * By +
As * Avz * Bz
);
float c = (
Mathf.Pow(Ax, 2) +
Mathf.Pow(Ay, 2) +
Mathf.Pow(Az, 2) -
Ax * Bx -
Ay * By -
Az * Bz +
Mathf.Pow(Bx, 2) +
Mathf.Pow(By, 2) +
Mathf.Pow(Bz, 2)
);
float t1 = (-b + Mathf.Pow((Mathf.Pow(b, 2) - (4 * a * c)), (1 / 2))) / (2 * a);
float t2 = (-b - Mathf.Pow((Mathf.Pow(b, 2) - (4 * a * c)), (1 / 2))) / (2 * a);
Debug.Log("t1 = " + t1 + "; t2 = " + t2);
if (t1 <= 0 || t1 == Mathf.Infinity || float.IsNaN(t1))
if (t2 <= 0 || t2 == Mathf.Infinity || float.IsNaN(t2))
return targetLocation;
else
t = t2;
else if (t2 <= 0 || t2 == Mathf.Infinity || float.IsNaN(t2) || t2 > t1)
t = t1;
else
t = t2;
Debug.Log("t = " + t);
Debug.Log("Bs = " + Bs);
float Bvx = (Ax - Bx + (t * As + Avx)) / (t * Mathf.Pow(Bs, 2));
float Bvy = (Ay - By + (t * As + Avy)) / (t * Mathf.Pow(Bs, 2));
float Bvz = (Az - Bz + (t * As + Avz)) / (t * Mathf.Pow(Bs, 2));
Vector3 Bv = new Vector3(Bvx, Bvy, Bvz);
Debug.Log("||Bv|| = (Should be 1) " + Bv.magnitude);
return Bv * Bs;
}
I followed the problem formulation as described by Gil Moshayof's answer, but found that there was an error in the simplification of the quadratic formula. When I did the derivation by hand I got a different solution.
The following is what worked for me when finding the intersect in 2D:
std::pair<double, double> find_2D_intersect(Vector3 sourcePos, double projSpd, Vector3 targetPos, double targetSpd, double targetHeading)
{
double P0x = targetPos.x;
double P0y = targetPos.y;
double s0 = targetSpd;
double V0x = std::cos(targetHeading);
double V0y = std::sin(targetHeading);
double P1x = sourcePos.x;
double P1y = sourcePos.y;
double s1 = projSpd;
// quadratic formula
double a = (s0 * s0)*((V0x * V0x) + (V0y * V0y)) - (s1 * s1);
double b = 2 * s0 * ((P0x * V0x) + (P0y * V0y) - (P1x * V0x) - (P1y * V0y));
double c = (P0x * P0x) + (P0y * P0y) + (P1x * P1x) + (P1y * P1y) - (2 * P1x * P0x) - (2 * P1y * P0y);
double t1 = (-b + std::sqrt((b * b) - (4 * a * c))) / (2 * a);
double t2 = (-b - std::sqrt((b * b) - (4 * a * c))) / (2 * a);
double t = choose_best_time(t1, t2);
double intersect_x = P0x + t * s0 * V0x;
double intersect_y = P0y + t * s0 * V0y;
return std::make_pair(intersect_x, intersect_y);
}
While working on SVG implementation for Internet Explorer to be based on its own VML format I came to a problem of translation of an SVG elliptical arc to an VML elliptical arc.
In VML an arc is given by: two angles for two points on ellipse and lengths of radiuses,
In SVG an arc is given by: two pairs of coordinates for two points on ellipse and sizes of ellipse boundary box
So, the question is: How to express angles of two points on ellipse to two pairs of their coordinates.
An intermediate question could be: How to find the center of an ellipse by coordinates of a pair of points on its curve.
Update: Let's have a precondition saying that an ellipse is normally placed (its radiuses are parallel to linear coordinate system axis), thus no rotation is applied.
Update: This question is not related to svg:ellipse element, rather to "a" elliptical arc command in svg:path element (SVG Paths: The elliptical arc curve commands)
So the solution is here:
The parametrized formula of an ellipse:
x = x0 + a * cos(t)
y = y0 + b * sin(t)
Let's put known coordinates of two points to it:
x1 = x0 + a * cos(t1)
x2 = x0 + a * cos(t2)
y1 = y0 + b * sin(t1)
y2 = y0 + b * sin(t2)
Now we have a system of equations with 4 variables: center of ellipse (x0/y0) and two angles t1, t2
Let's subtract equations in order to get rid of center coordinates:
x1 - x2 = a * (cos(t1) - cos(t2))
y1 - y2 = b * (sin(t1) - sin(t2))
This can be rewritten (with product-to-sum identities formulas) as:
(x1 - x2) / (2 * a) = sin((t1 + t2) / 2) * sin((t1 - t2) / 2)
(y2 - y1) / (2 * b) = cos((t1 + t2) / 2) * sin((t1 - t2) / 2)
Let's replace some of the equations:
r1: (x1 - x2) / (2 * a)
r2: (y2 - y1) / (2 * b)
a1: (t1 + t2) / 2
a2: (t1 - t2) / 2
Then we get simple equations system:
r1 = sin(a1) * sin(a2)
r2 = cos(a1) * sin(a2)
Dividing first equation by second produces:
a1 = arctan(r1/r2)
Adding this result to the first equation gives:
a2 = arcsin(r2 / cos(arctan(r1/r2)))
Or, simple (using compositions of trig and inverse trig functions):
a2 = arcsin(r2 / (1 / sqrt(1 + (r1/r2)^2)))
or even more simple:
a2 = arcsin(sqrt(r1^2 + r2^2))
Now the initial four-equations system can be resolved with easy and all angles as well as eclipse center coordinates can be found.
The elliptical curve arc link you posted includes a link to elliptical arc implementation notes.
In there, you will find the equations for conversion from endpoint to centre parameterisation.
Here is my JavaScript implementation of those equations, taken from an interactive demo of elliptical arc paths, using Sylvester.js to perform the matrix and vector calculations.
// Calculate the centre of the ellipse
// Based on http://www.w3.org/TR/SVG/implnote.html#ArcConversionEndpointToCenter
var x1 = 150; // Starting x-point of the arc
var y1 = 150; // Starting y-point of the arc
var x2 = 400; // End x-point of the arc
var y2 = 300; // End y-point of the arc
var fA = 1; // Large arc flag
var fS = 1; // Sweep flag
var rx = 100; // Horizontal radius of ellipse
var ry = 50; // Vertical radius of ellipse
var phi = 0; // Angle between co-ord system and ellipse x-axes
var Cx, Cy;
// Step 1: Compute (x1′, y1′)
var M = $M([
[ Math.cos(phi), Math.sin(phi)],
[-Math.sin(phi), Math.cos(phi)]
]);
var V = $V( [ (x1-x2)/2, (y1-y2)/2 ] );
var P = M.multiply(V);
var x1p = P.e(1); // x1 prime
var y1p = P.e(2); // y1 prime
// Ensure radii are large enough
// Based on http://www.w3.org/TR/SVG/implnote.html#ArcOutOfRangeParameters
// Step (a): Ensure radii are non-zero
// Step (b): Ensure radii are positive
rx = Math.abs(rx);
ry = Math.abs(ry);
// Step (c): Ensure radii are large enough
var lambda = ( (x1p * x1p) / (rx * rx) ) + ( (y1p * y1p) / (ry * ry) );
if(lambda > 1)
{
rx = Math.sqrt(lambda) * rx;
ry = Math.sqrt(lambda) * ry;
}
// Step 2: Compute (cx′, cy′)
var sign = (fA == fS)? -1 : 1;
// Bit of a hack, as presumably rounding errors were making his negative inside the square root!
if((( (rx*rx*ry*ry) - (rx*rx*y1p*y1p) - (ry*ry*x1p*x1p) ) / ( (rx*rx*y1p*y1p) + (ry*ry*x1p*x1p) )) < 1e-7)
var co = 0;
else
var co = sign * Math.sqrt( ( (rx*rx*ry*ry) - (rx*rx*y1p*y1p) - (ry*ry*x1p*x1p) ) / ( (rx*rx*y1p*y1p) + (ry*ry*x1p*x1p) ) );
var V = $V( [rx*y1p/ry, -ry*x1p/rx] );
var Cp = V.multiply(co);
// Step 3: Compute (cx, cy) from (cx′, cy′)
var M = $M([
[ Math.cos(phi), -Math.sin(phi)],
[ Math.sin(phi), Math.cos(phi)]
]);
var V = $V( [ (x1+x2)/2, (y1+y2)/2 ] );
var C = M.multiply(Cp).add(V);
Cx = C.e(1);
Cy = C.e(2);
An ellipse cannot be defined by only two points. Even a circle (a special cased ellipse) is defined by three points.
Even with three points, you would have infinite ellipses passing through these three points (think: rotation).
Note that a bounding box suggests a center for the ellipse, and most probably assumes that its major and minor axes are parallel to the x,y (or y,x) axes.
The intermediate question is fairly easy... you don't. You work out the centre of an ellipse from the bounding box (namely, the centre of the box is the centre of the ellipse, as long as the ellipse is centred in the box).
For your first question, I'd look at the polar form of the ellipse equation, which is available on Wikipedia. You would need to work out the eccentricity of the ellipse as well.
Or you could brute force the values from the bounding box... work out if a point lies on the ellipse and matches the angle, and iterate through every point in the bounding box.
TypeScript implementation based on the answer from Rikki.
Default DOMMatrix and DOMPoint are used for the calculations (Tested in the latest Chrome v.80) instead of the external library.
ellipseCenter(
x1: number,
y1: number,
rx: number,
ry: number,
rotateDeg: number,
fa: number,
fs: number,
x2: number,
y2: number
): DOMPoint {
const phi = ((rotateDeg % 360) * Math.PI) / 180;
const m = new DOMMatrix([
Math.cos(phi),
-Math.sin(phi),
Math.sin(phi),
Math.cos(phi),
0,
0,
]);
let v = new DOMPoint((x1 - x2) / 2, (y1 - y2) / 2).matrixTransform(m);
const x1p = v.x;
const y1p = v.y;
rx = Math.abs(rx);
ry = Math.abs(ry);
const lambda = (x1p * x1p) / (rx * rx) + (y1p * y1p) / (ry * ry);
if (lambda > 1) {
rx = Math.sqrt(lambda) * rx;
ry = Math.sqrt(lambda) * ry;
}
const sign = fa === fs ? -1 : 1;
const div =
(rx * rx * ry * ry - rx * rx * y1p * y1p - ry * ry * x1p * x1p) /
(rx * rx * y1p * y1p + ry * ry * x1p * x1p);
const co = sign * Math.sqrt(Math.abs(div));
// inverse matrix b and c
m.b *= -1;
m.c *= -1;
v = new DOMPoint(
((rx * y1p) / ry) * co,
((-ry * x1p) / rx) * co
).matrixTransform(m);
v.x += (x1 + x2) / 2;
v.y += (y1 + y2) / 2;
return v;
}
Answering part of the question with code
How to find the center of an ellipse by coordinates of a pair of points on its curve.
This is a TypeScript function which is based on the excellent accepted answer by Sergey Illinsky above (which ends somewhat halfway through, IMHO). It calculates the center of an ellipse with given radii, given the condition that both provided points a and b must lie on the circumference of the ellipse. Since there are (almost) always two solutions to this problem, the code choses the solution that places the ellipse "above" the two points:
(Note that the ellipse must have major and minor axis parallel to the horizontal/vertical)
/**
* We're in 2D, so that's what our vertors look like
*/
export type Point = [number, number];
/**
* Calculates the vector that connects the two points
*/
function deltaXY (from: Point, to: Point): Point {
return [to[0]-from[0], to[1]-from[1]];
}
/**
* Calculates the sum of an arbitrary amount of vertors
*/
function vecAdd (...vectors: Point[]): Point {
return vectors.reduce((acc, curr) => [acc[0]+curr[0], acc[1]+curr[1]], [0, 0]);
}
/**
* Given two points a and b, as well as ellipsis radii rX and rY, this
* function calculates the center-point of the ellipse, so that it
* is "above" the two points and has them on the circumference
*/
function topLeftOfPointsCenter (a: Point, b: Point, rX: number, rY: number): Point {
const delta = deltaXY(a, b);
// Sergey's work leads up to a simple system of liner equations.
// Here, we calculate its general solution for the first of the two angles (t1)
const A = Math.asin(Math.sqrt((delta[0]/(2*rX))**2+(delta[1]/(2*rY))**2));
const B = Math.atan(-delta[0]/delta[1] * rY/rX);
const alpha = A + B;
// This may be the new center, but we don't know to which of the two
// solutions it belongs, yet
let newCenter = vecAdd(a, [
rX * Math.cos(alpha),
rY * Math.sin(alpha)
]);
// Figure out if it is the correct solution, and adjusting if not
const mean = vecAdd(a, [delta[0] * 0.5, delta[1] * 0.5]);
const factor = mean[1] > newCenter[1] ? 1 : -1;
const offMean = deltaXY(mean, newCenter);
newCenter = vecAdd(mean, [offMean[0] * factor, offMean[1] * factor]);
return newCenter;
}
This function does not check if a solution is possible, meaning whether the radii provided are large enough to even connect the two points!