Here is the problem. We have two points (spheres) in xyz, with this info:
1- x,y,z => The center of the object is currently located at
2- r => The collision radius of the object
3- Vx, Vy, Vz => object is traveling along the vector. If that vector is (0,0,0), the object is stationary.
Note: The radii and positions are in meters and velocities are measured in meters per second.
Question: For each test, output a single line containing the time (in seconds) since the start of the test at which the two objects will first collide. If they never collide, print No collision instead.
I want to know about the formula of calculation this time. Any idea would be appreciated.
Examples:
1-
xyz(1): -7 5 0
v(1): -1 0 3
r(1): 3
xyz(2): 10 7 -6
v(2): -2 0 4
r(2): 6
t: 8.628 // this is the answer
2-
xyz(1): 10 3 -10
v(1): -9 3 -8
r(1): 5
xyz(2): 2 0 0
v(2): 6
r(2): -4 3 -10
t: 0.492 // this is the answer
To simplify problem, let us use Halileo's principle. Consider the first object as base point, so the second objects moves relative to it.
Put the first object position in coordinate origin.
Subtract the first object initial coordinates from the second one coordinates, do the same for velocity components
x2_0 = x2_0 - x1_0 (same for y,z)
Vx2 = Vx2 - Vx1 (same for y,z)
Now we have second center coordinates against time
x = x2_0 + Vx2 * t
y = y2_0 + Vy2 * t
z = z2_0 + Vz2 * t
and squared distance to origin:
dd = x*x + y*y + z*z =
(x2_0 + Vx2 * t)^2 + ... =
x2_0^2 + 2*x2_0*Vx2*t + Vx2^2*t^2 + ...
and we need to calculate when dd becomes equal to squared radius sum (r1+r2)^2
t^2 * (Vx2^2+Vy2^2+Vz2^2) + t*(2*x2_0*Vx2+2*y2_0*Vy2+2*z2_0*Vz2) +
x2_0^2 + y2_0^2 + y2_0^2 - (r1+r2)^2 = 0
Solve this quadratic equation for t, get 0,1 or 2 solutions.
Case of 0 solutions - no collision
Case of 1 solution with positive t - moment of touching
Case of two solutions - get smaller positive t for the moment of collision.
Negative values of t mean collision "in the past", before the start of the test
Quick test in Python (ideone)
from math import sqrt, isclose
def collisiontime(x1,y1,z1,vx1,vy1,vz1,r1, x2,y2,z2,vx2,vy2,vz2,r2):
x2 -= x1
y2 -= y1
z2 -= z1
vx2 -= vx1
vy2 -= vy1
vz2 -= vz1
a = vx2**2 + vy2**2 + vz2**2
b = 2*x2*vx2 + 2*y2*vy2 + 2*z2*vz2
c = x2**2 + y2**2 + z2**2 - (r1+r2)**2
D = b**2-4*a*c
if D < 0:
return None
if isclose(D, 0):
return -b/2/a
return (-b - sqrt(D)) / 2 /a, (-b + sqrt(D)) / 2 /a
print(collisiontime(0, 0, 0, 2, 0, 0, 2, 25, 0, 0, -3, 0, 0, 3)) # 1=> <=2
print(collisiontime(0, 0, 0, 2, 0, 0, 2, 25, 5, 0, 1, 0, 0, 3)) # 1==> 2=> chase with touching
print(collisiontime(-7, 5, 0,-1, 0, 3, 3, 10, 7, -6, -2, 0, 4, 6))
print(collisiontime(10, 3, -10,-9, 3, -8,5, 2, 0, 0, -4, 3, -10, 6))
(4.0, 6.0)
25.0
(8.627718676730986, 14.372281323269014)
(0.4917797757201004, 3.646151258762658)
Related
I saw few similar post to this question, but none that provided a concrete final working solution.
I'm working with OptiTrack with python, Motive 2.2.0, NatNet SDK 4.0 using the NatNetClient from the examples provided with the SDK.
The coordinates system is such that Y is Up, X is backward and Z is left.
I want to translate the quaternion to a coordinates system in which X is forward, Y is right (or left, the more simple one) and Z is up.
I'm getting the quaternion qx, qy, qz, qw values, I think that in this order but I'm not sure (if you can find it in the documentation of Motive/OptiTrack it also could help).
Now I'm trying by a plenty of similar ways that I think should work to get the Euler angles: pitch, roll and yaw, and then check for the three rotations in which I should get 0 to -180 and then to 180 and back to 0 (or vice versa), but it is always results in roll direction which goes from 0 to 90 then back to 0 (positive grow and then positive decrease) and then to -90 and then again back to 0 (negative decrease and then negative grow).
Correct me on this too, but I that is the result that serves as the sanity check for assurance the transformation to Euler was done correctly, right ?
First I take the pose and the quat and create an SO(3) matrix (just for convenience) :
def pos_quat2SE(quat_data):
# Assumed quat_data order is (pos, quat)
SO = R.from_quat(quat_data[3:7]).as_matrix()
SE = np.matrix(np.eye(4))
SE[0:3,0:3] = np.matrix(SO)
SE[0:3,3] = np.matrix(quat_data[0:3]).T
return SE_motive
where quat_data is a simple concatenation of pos (3 values) and quat (4 values) as mentioned.
I tried to use scipy function:
from scipy.spatial.transform import Rotation as R
euler_transformed = R.from_matrix(SE_motive[0:3, 0:3]).as_euler('zyx', degrees=False)
but I'm not sure what should be the right argument for as_euler.
Also tried to use the following approach using this auxiliary function:
def SE_motive2transoform(SE_motive):
T_Yup2NED_inv = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T_Yup2NED = invert_SE(T_Yup2NED_inv)
SE_transformed = SE_motive # T_Yup2NED
return SE_transformed
The next two tries gave the same result:
using this functions which should be equivalent:
def euler_from_quaternion(x, y, z, w):
"""
Convert a quaternion into euler angles (roll, pitch, yaw)
roll is rotation around x in radians (counterclockwise)
pitch is rotation around y in radians (counterclockwise)
yaw is rotation around z in radians (counterclockwise)
"""
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return roll_x, pitch_y, yaw_z # in radians
def quaternion_to_rotation_matrix(q):
"""Return a 3x3 rotation matrix representing the orientation specified by a quaternion in x,y,z,w format.
The matrix is a Python list of lists.
"""
x = q[0]
y = q[1]
z = q[2]
w = q[3]
return [[w * w + x * x - y * y - z * z, 2 * (x * y - w * z), 2 * (x * z + w * y)],
[2 * (x * y + w * z), w * w - x * x + y * y - z * z, 2 * (y * z - w * x)],
[2 * (x * z - w * y), 2 * (y * z + w * x), w * w - x * x - y * y + z * z]]
on the input new_quat = np.vstack([quato[0], -quato[2], quato[1], quato[3]]) where qauto is the returned quaternion from the motive system in its mentioned above coordinate system. As much as I understand rearrangement of the quaternion values in that way should give me them in an xyz coordinate system and then I should been able to use the above function or even as_euler with xyz argument and etc. but it didn't work.
What is the shortest, working and elegant way to achieve the transform with the sanity check working of course ?
can it be done in that fashion:
def SE_motive2transoform(SE_motive):
T_Yup2NED_inv = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T_Yup2NED = invert_SE(T_Yup2NED_inv)
SE_transformed = SE_motive # T_Yup2NED
return SE_transformed
Thank you in advance.
I transformed a vector which lays on the transformation matrix's eigen space using Object3D.applyMatrix4. I expected it not to move because the eigen value was 1, or at least stay on the eigen span, but it moved to an unexpected place.
I've made an sample project: stackblitz
But below is probably all the code you would need to examine
let pointGeo = new THREE.Geometry()
pointGeo.vertices = [new THREE.Vector3(1,-1,1)]
let pmat = new THREE.PointsMaterial({color: 0x00f0f0, size: 0.3})
let pointObj = new THREE.Points(pointGeo, pmat)
let matrix = new THREE.Matrix4()
matrix.set( 1, 3, 3, 0,
-3, -5, -3, 0,
3, 3, 1, 0,
0, 0, 0, 1)
pointObj.applyMatrix4(matrix)
Specifically I'm transforming a point (1, -1, 1) (where the purple square is) with a matrix. Multiplication by the matrix shouldn't change the value. (double checked by hand calculating) But it moves it to where the blue square is.
Is there other stages where some other transformation is applied? What am I missing here?
I've found that Object3D.applyMatrix4 wasn't the right function for explicitly defining local matrix. I tried setting Object3D.matrix property directly and it worked. I changed code like this:
o.matrix = mat
o.matrixAutoUpdate = false
o.updateMatrixWorld(true)
here is the source for applyMatrix4... are you hand applying your matrix exactly like this:
applyMatrix4 ( m ) {
var x = this.x, y = this.y, z = this.z;
var e = m.elements;
var w = 1 / ( e[ 3 ] * x + e[ 7 ] * y + e[ 11 ] * z + e[ 15 ] );
this.x = ( e[ 0 ] * x + e[ 4 ] * y + e[ 8 ] * z + e[ 12 ] ) * w;
this.y = ( e[ 1 ] * x + e[ 5 ] * y + e[ 9 ] * z + e[ 13 ] ) * w;
this.z = ( e[ 2 ] * x + e[ 6 ] * y + e[ 10 ] * z + e[ 14 ] ) * w;
return this;
},
```
I have a 3x3 Convolution Function defined like this
conv(x, y) = 0;
conv(x, y) += kernel(r.x, r.y) * in(x + r.x - 1, y + r.y - 1);
Size of the input buffer is 16 x 16
If I want to execute it with padding I can directly do
in = Halide::BoundaryConditions::constant_exterior(in_buffer, 0, 0, 16, 0, 16)
But I have to execute without padding and so I am trying to manually set the bounds on the function like this
conv.bound(x, 1, 14);
conv.bound(y, 1, 14);
This returns an error message
Error:
Bounds given for convolution in y (from 1 to 14) do not cover required region (from 0 to 15)
What should I do to set bounds on a Var in Func?
I think you need not to manually set the bounds using the *.bound function. Try this one:
Halide::Func conv("conv"), kernelF("kernel"), in("in");
Halide::Var x("x"), y("y");
Halide::RDom r(0, 3, 0, 3,"r");
in = Halide::BoundaryConditions::constant_exterior(in_buffer, 0,
0, 16, 0, 16);
kernelF = Halide::BoundaryConditions::constant_exterior(kernel_buffer, 0,
0, 3, 0, 3);
conv(x, y) = 0.0f;
conv(x, y) += kernelF(r.x, r.y) * in(x + r.x, y + r.y);
//conv.print_loop_nest();
Halide::Buffer<float_t> outputBuf = conv.realize(14, 14);
Look, we can set the bounds directly in *.realize() arguments, i.e. 14=16-3+1; Also, note that the convolution anchors are at the top-left of kernels.
Okay algebra and trig are not my strong suit by any means so here is what I need to do.
I have a circle which is measured in degrees from +180 to -180 (360 total)
Given the center point of the circle stays the same, Cx , Cy.
The angle varies from -180 to +180
I need to locate a point that regardless the given angle is + 3 units away that is at the 90 degree position and the 270 degree position (from the given degrees)
So like...
Angle = 0
Point 1 -> x = 0, y -3
Point 2 -> x = 0, y + 3
And if the angle was say 90 (provided its measured Clockwise)
Point 1 -> x = -3, y = 0
Point 2 -> x = 3, y = 0
What I need is a forumla that will accept Angle, then tell me what my x/y should be 3 units away from the origin.
I have tried: EDIT Updated to double precision using Java.
`double x = Cx + 3 * Math.cos((d + 90) * Math.PI / 180);'
'double y = Cy + 3 * Math.sin((d + 90) * Math.PI / 180);`
this gives me mixed results, I mean sometimes it's where I think it should be and other times its quite wrong.
Assuming Cx = 0.500, Cy = 0.500
Sample Data: Result:
Deg = 0 x = 2 / y = 5
Deg = 90 x = -1 / y = 2
Deg = 125 x = -0.457 / y = 0.297
Deg = 159 x = 0.924 / y = -0.800
I realize I am only calculating one point at this point but do you have any suggestions on how to get the first point working? at say 90 degrees from whatever degree I start with?
x = Cx + r * Math.cos( (d+90) * Math.PI / 180 );
y = Cy + r * Math.sin( (d+90) * Math.PI / 180 );
Seems that this is the correct formula for what I was trying to accomplish. This will take any value for Cx/Cy's origin add the Radius r, then calculate the degrees + 90 and convert to radians.. Once all that magic takes place, you're left with an x/y coord that is 90 degrees of where you started.
I am trying to add this parametric plot only in the z-axis (right now when I add it expands in the x,y, and z), the effect of this summation would be addition of amplitudes of the sine waves. Here is what I have now. http://imgur.com/j9hN7VR
Here is the code I am using to implement it:
frequency = 1000;
speed = 13397.2441;
wavelength = speed/frequency;
s = (r - 2);
t = (r - 4);
u = (r - 6);
v = (r - 8);
ParametricPlot3D[{{r*Cos[q] - 4, r*Sin[q], Sin[(2*Pi)/wavelength*(r)]},
{s*Cos[q] - 2, s*Sin[q], Sin[(2*Pi)/wavelength*(s + wavelength/4 - 1)]},
{t*Cos[q], t*Sin[q], Sin[(2*Pi)/wavelength*(t + wavelength/4 + 0.5)]},
{u*Cos[q] + 2, u*Sin[q], Sin[(2*Pi)/wavelength*(u + wavelength/4 + 1.65)]},
{v*Cos[q] + 4, v*Sin[q], Sin[(2*Pi)/wavelength*(v + wavelength/4 + 3.5)]}},
{r, 0, 25}, {q, 0, Pi}, PlotPoints -> 30, Mesh -> 20, PlotRange -> {{-25, 25}, {0, 35}, {-6, 6}}]
Any suggestions would be greatly appreciated!
Unfortunately I could not find an answer for this, so I ended up just simulating in MATLAB instead by generating all values over the field (in a matrix) and then summing as I was trying to do here.