I'm looking for a languages for querying CBOR, like JsonPath or jq but for CBOR binary format. I don't want to convert from CBOR to JSON because some CBOR type is not existed in JSON, and performance issue.
The C++ library jsoncons allows you to query CBOR with JSONPath, for example,
#include <jsoncons/json.hpp>
#include <jsoncons_ext/cbor/cbor.hpp>
#include <jsoncons_ext/jsonpath/json_query.hpp>
#include <iomanip>
using namespace jsoncons; // For convenience
int main()
{
std::vector<uint8_t> v = {0x85,0xfa,0x40,0x0,0x0,0x0,0xfb,0x3f,0x12,0x9c,0xba,0xb6,0x49,0xd3,0x89,0xc3,0x49,0x1,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0xc4,0x82,0x38,0x1c,0xc2,0x4d,0x1,0x8e,0xe9,0xf,0xf6,0xc3,0x73,0xe0,0xee,0x4e,0x3f,0xa,0xd2,0xc5,0x82,0x20,0x3};
/*
85 -- Array of length 5
fa -- float
40a00000 -- 5.0
fb -- double
3f129cbab649d389 -- 0.000071
c3 -- Tag 3 (negative bignum)
49 -- Byte string value of length 9
010000000000000000
c4 -- Tag 4 (decimal fraction)
82 -- Array of length 2
38 -- Negative integer of length 1
1c -- -29
c2 -- Tag 2 (positive bignum)
4d -- Byte string value of length 13
018ee90ff6c373e0ee4e3f0ad2
c5 -- Tag 5 (bigfloat)
82 -- Array of length 2
20 -- -1
03 -- 3
*/
// Decode to a json value (despite its name, it is not JSON specific.)
json j = cbor::decode_cbor<json>(v);
// Serialize to JSON
std::cout << "(1)\n";
std::cout << pretty_print(j);
std::cout << "\n\n";
// as<std::string>() and as<double>()
std::cout << "(2)\n";
std::cout << std::dec << std::setprecision(15);
for (const auto& item : j.array_range())
{
std::cout << item.as<std::string>() << ", " << item.as<double>() << "\n";
}
std::cout << "\n";
// Query with JSONPath
std::cout << "(3)\n";
json result = jsonpath::json_query(j,"$.[?(# < 1.5)]");
std::cout << pretty_print(result) << "\n\n";
// Encode result as CBOR
std::vector<uint8_t> val;
cbor::encode_cbor(result,val);
std::cout << "(4)\n";
for (auto c : val)
{
std::cout << std::hex << std::setprecision(2) << std::setw(2)
<< std::setfill('0') << static_cast<int>(c);
}
std::cout << "\n\n";
/*
83 -- Array of length 3
fb -- double
3f129cbab649d389 -- 0.000071
c3 -- Tag 3 (negative bignum)
49 -- Byte string value of length 9
010000000000000000
c4 -- Tag 4 (decimal fraction)
82 -- Array of length 2
38 -- Negative integer of length 1
1c -- -29
c2 -- Tag 2 (positive bignum)
4d -- Byte string value of length 13
018ee90ff6c373e0ee4e3f0ad2
*/
}
Output:
(1)
[
2.0,
7.1e-05,
"-18446744073709551617",
"1.23456789012345678901234567890",
[-1, 3]
]
(2)
2.0, 2
7.1e-05, 7.1e-05
-18446744073709551617, -1.84467440737096e+19
1.23456789012345678901234567890, 1.23456789012346
1.5, 1.5
(3)
[
7.1e-05,
"-18446744073709551617",
"1.23456789012345678901234567890"
]
(4)
83fb3f129cbab649d389c349010000000000000000c482381cc24d018ee90ff6c373e0ee4e3f0ad2
Sure, you can use any general purpose programming language for querying CBOR, for example JavaScript might be a good choice. But if you are looking for a "query language" like JsonPath, I'm not aware of any specifically developed for CBOR.
Quaternions represent rotations - they don't include information about scaling or mirroring. However it is still possible to mirror the effect of a rotation.
Consider a mirroring on the x-y-plane (we can also call it a mirroring along the z-axis). A rotation around the x-axis mirrored on the x-y-plane would be negated. Likewise with a rotation around the y axis. However, a rotation around the z-axis would be left unchanged.
Another example: 90º rotation around axis (1,1,1) mirrored in the x-y plane would give -90º rotation around (1,1,-1). To aid the intuition, if you can visualize a depiction of the axis and a circular arrow indicating the rotation, then mirroring that visualization indicates what the new rotation should be.
I have found a way to calculate this mirroring of the rotation, like this:
Get the angle-axis representation of the quaternion.
For each of the axes x, y, and z.
If the scaling is negative (mirrored) along that axis:
Negate both angle and axis.
Get the updated quaternion from the modified angle and axis.
This only supports mirroring along the primary axes, x, y, and z, since that's all I need. It works for arbitrary rotations though.
However, the conversions from quaternion to angle-axis and back from angle-axis to quaternion are expensive. I'm wondering if there's a way to do the conversion directly on the quaternion itself, but my comprehension of quaternion math is not sufficient to get anywhere myself.
(Posted on StackOverflow rather than math-related forums due to the importance of a computationally efficient method.)
I just spent quite some time on figuring out a clear answer to this question, so I am posting it here for the record.
Introduction
As was noted in other answers, a mirror effect cannot be represented as a rotation. However, given a rotation R1to2 from a coordinate frame C1 to a coordinate frame C2, we may be interested in efficiently computing the equivalent rotation when applying the same mirror effect to C1 and C2 (e.g. I was facing the problem of converting an input quaternion, given in a left-handed coordinate frame, into the quaternion representing the same rotation but in a right-handed coordinate frame).
In terms of rotation matrices, this can be thought of as follows:
R_mirroredC1_to_mirroredC2 = M_mirrorC2 * R_C1_to_C2 * M_mirrorC1
Here, both R_C1_to_C2 and R_mirroredC1_to_mirroredC2 represent valid rotations, so when dealing with quaternions, how do you efficiently compute q_mirroredC1_to_mirroredC2 from q_C1_to_C2?
Solution
The following assumes that q_C1_to_C2=[w,x,y,z]:
if C1 and C2 are mirrored along the X-axis (i.e. M_mirrorC1=M_mirrorC2=diag_3x3(-1,1,1)) then q_mirroredC1_to_mirroredC2=[w,x,-y,-z]
if C1 and C2 are mirrored along the Y-axis (i.e. M_mirrorC1=M_mirrorC2=diag_3x3(1,-1,1)) then q_mirroredC1_to_mirroredC2=[w,-x,y,-z]
if C1 and C2 are mirrored along the Z-axis (i.e. M_mirrorC1=M_mirrorC2=diag_3x3(1,1,-1)) then q_mirroredC1_to_mirroredC2=[w,-x,-y,z]
When considering different mirrored axes for the C1 and C2, we have the following:
if C1 is mirrored along the X-axis and C2 along the Y-axis (i.e. M_mirrorC1=diag_3x3(-1,1,1) & M_mirrorC2=diag_3x3(1,-1,1)) then q_mirroredC1_to_mirroredC2=[z,y,x,w]
if C1 is mirrored along the X-axis and C2 along the Z-axis (i.e. M_mirrorC1=diag_3x3(-1,1,1) & M_mirrorC2=diag_3x3(1,1,-1)) then q_mirroredC1_to_mirroredC2=[-y,z,-w,x]
if C1 is mirrored along the Y-axis and C2 along the X-axis (i.e. M_mirrorC1=diag_3x3(1,-1,1) & M_mirrorC2=diag_3x3(-1,1,1)) then q_mirroredC1_to_mirroredC2=[z,-y,-x,w]
if C1 is mirrored along the Y-axis and C2 along the Z-axis (i.e. M_mirrorC1=diag_3x3(1,-1,1) & M_mirrorC2=diag_3x3(1,1,-1)) then q_mirroredC1_to_mirroredC2=[x,w,z,y]
if C1 is mirrored along the Z-axis and C2 along the X-axis (i.e. M_mirrorC1=diag_3x3(1,1,-1) & M_mirrorC2=diag_3x3(-1,1,1)) then q_mirroredC1_to_mirroredC2=[y,z,w,x]
if C1 is mirrored along the Z-axis and C2 along the Y-axis (i.e. M_mirrorC1=diag_3x3(1,1,-1) & M_mirrorC2=diag_3x3(1,-1,1)) then q_mirroredC1_to_mirroredC2=[x,w,-z,-y]
Test program
Here is a small c++ program based on OpenCV to test all this:
#include <opencv2/opencv.hpp>
#define CST_PI 3.1415926535897932384626433832795
// Random rotation matrix uniformly sampled from SO3 (see "Fast random rotation matrices" by J.Arvo)
cv::Matx<double,3,3> get_random_rotmat()
{
double theta1 = 2*CST_PI*cv::randu<double>();
double theta2 = 2*CST_PI*cv::randu<double>();
double x3 = cv::randu<double>();
cv::Matx<double,3,3> R(std::cos(theta1),std::sin(theta1),0,-std::sin(theta1),std::cos(theta1),0,0,0,1);
cv::Matx<double,3,1> v(std::cos(theta2)*std::sqrt(x3),std::sin(theta2)*std::sqrt(x3),std::sqrt(1-x3));
return -1*(cv::Matx<double,3,3>::eye()-2*v*v.t())*R;
}
cv::Matx<double,4,1> rotmat2quatwxyz(const cv::Matx<double,3,3> &R)
{
// Implementation from Ceres 1.10
const double trace = R(0,0) + R(1,1) + R(2,2);
cv::Matx<double,4,1> quat_wxyz;
if (trace >= 0.0) {
double t = sqrt(trace + 1.0);
quat_wxyz(0) = 0.5 * t;
t = 0.5 / t;
quat_wxyz(1) = (R(2,1) - R(1,2)) * t;
quat_wxyz(2) = (R(0,2) - R(2,0)) * t;
quat_wxyz(3) = (R(1,0) - R(0,1)) * t;
} else {
int i = 0;
if (R(1, 1) > R(0, 0))
i = 1;
if (R(2, 2) > R(i, i))
i = 2;
const int j = (i + 1) % 3;
const int k = (j + 1) % 3;
double t = sqrt(R(i, i) - R(j, j) - R(k, k) + 1.0);
quat_wxyz(i + 1) = 0.5 * t;
t = 0.5 / t;
quat_wxyz(0) = (R(k,j) - R(j,k)) * t;
quat_wxyz(j + 1) = (R(j,i) + R(i,j)) * t;
quat_wxyz(k + 1) = (R(k,i) + R(i,k)) * t;
}
// Check that the w element is positive
if(quat_wxyz(0)<0)
quat_wxyz *= -1; // quat and -quat represent the same rotation, but to make quaternion comparison easier, we always use the one with positive w
return quat_wxyz;
}
cv::Matx<double,4,1> apply_quaternion_trick(const unsigned int item_permuts[4], const int sign_flips[4], const cv::Matx<double,4,1>& quat_wxyz)
{
// Flip the sign of the x and z components
cv::Matx<double,4,1> quat_flipped(sign_flips[0]*quat_wxyz(item_permuts[0]),sign_flips[1]*quat_wxyz(item_permuts[1]),sign_flips[2]*quat_wxyz(item_permuts[2]),sign_flips[3]*quat_wxyz(item_permuts[3]));
// Check that the w element is positive
if(quat_flipped(0)<0)
quat_flipped *= -1; // quat and -quat represent the same rotation, but to make quaternion comparison easier, we always use the one with positive w
return quat_flipped;
}
void detect_quaternion_trick(const cv::Matx<double,4,1> &quat_regular, const cv::Matx<double,4,1> &quat_flipped, unsigned int item_permuts[4], int sign_flips[4])
{
if(abs(quat_regular(0))==abs(quat_flipped(0))) {
item_permuts[0]=0;
sign_flips[0] = (quat_regular(0)/quat_flipped(0)>0 ? 1 : -1);
}
else if(abs(quat_regular(0))==abs(quat_flipped(1))) {
item_permuts[1]=0;
sign_flips[1] = (quat_regular(0)/quat_flipped(1)>0 ? 1 : -1);
}
else if(abs(quat_regular(0))==abs(quat_flipped(2))) {
item_permuts[2]=0;
sign_flips[2] = (quat_regular(0)/quat_flipped(2)>0 ? 1 : -1);
}
else if(abs(quat_regular(0))==abs(quat_flipped(3))) {
item_permuts[3]=0;
sign_flips[3] = (quat_regular(0)/quat_flipped(3)>0 ? 1 : -1);
}
if(abs(quat_regular(1))==abs(quat_flipped(0))) {
item_permuts[0]=1;
sign_flips[0] = (quat_regular(1)/quat_flipped(0)>0 ? 1 : -1);
}
else if(abs(quat_regular(1))==abs(quat_flipped(1))) {
item_permuts[1]=1;
sign_flips[1] = (quat_regular(1)/quat_flipped(1)>0 ? 1 : -1);
}
else if(abs(quat_regular(1))==abs(quat_flipped(2))) {
item_permuts[2]=1;
sign_flips[2] = (quat_regular(1)/quat_flipped(2)>0 ? 1 : -1);
}
else if(abs(quat_regular(1))==abs(quat_flipped(3))) {
item_permuts[3]=1;
sign_flips[3] = (quat_regular(1)/quat_flipped(3)>0 ? 1 : -1);
}
if(abs(quat_regular(2))==abs(quat_flipped(0))) {
item_permuts[0]=2;
sign_flips[0] = (quat_regular(2)/quat_flipped(0)>0 ? 1 : -1);
}
else if(abs(quat_regular(2))==abs(quat_flipped(1))) {
item_permuts[1]=2;
sign_flips[1] = (quat_regular(2)/quat_flipped(1)>0 ? 1 : -1);
}
else if(abs(quat_regular(2))==abs(quat_flipped(2))) {
item_permuts[2]=2;
sign_flips[2] = (quat_regular(2)/quat_flipped(2)>0 ? 1 : -1);
}
else if(abs(quat_regular(2))==abs(quat_flipped(3))) {
item_permuts[3]=2;
sign_flips[3] = (quat_regular(2)/quat_flipped(3)>0 ? 1 : -1);
}
if(abs(quat_regular(3))==abs(quat_flipped(0))) {
item_permuts[0]=3;
sign_flips[0] = (quat_regular(3)/quat_flipped(0)>0 ? 1 : -1);
}
else if(abs(quat_regular(3))==abs(quat_flipped(1))) {
item_permuts[1]=3;
sign_flips[1] = (quat_regular(3)/quat_flipped(1)>0 ? 1 : -1);
}
else if(abs(quat_regular(3))==abs(quat_flipped(2))) {
item_permuts[2]=3;
sign_flips[2] = (quat_regular(3)/quat_flipped(2)>0 ? 1 : -1);
}
else if(abs(quat_regular(3))==abs(quat_flipped(3))) {
item_permuts[3]=3;
sign_flips[3] = (quat_regular(3)/quat_flipped(3)>0 ? 1 : -1);
}
}
int main(int argc, char **argv)
{
cv::Matx<double,3,3> M_xflip(-1,0,0,0,1,0,0,0,1);
cv::Matx<double,3,3> M_yflip(1,0,0,0,-1,0,0,0,1);
cv::Matx<double,3,3> M_zflip(1,0,0,0,1,0,0,0,-1);
// Let the user choose the configuration
char im,om;
std::cout << "Enter the axis (x,y,z) along which input ref is flipped:" << std::endl;
std::cin >> im;
std::cout << "Enter the axis (x,y,z) along which output ref is flipped:" << std::endl;
std::cin >> om;
cv::Matx<double,3,3> M_iflip,M_oflip;
if(im=='x') M_iflip=M_xflip;
else if(im=='y') M_iflip=M_yflip;
else if(im=='z') M_iflip=M_zflip;
if(om=='x') M_oflip=M_xflip;
else if(om=='y') M_oflip=M_yflip;
else if(om=='z') M_oflip=M_zflip;
// Generate random quaternions until we find one where no two elements are equal
cv::Matx<double,3,3> R;
cv::Matx<double,4,1> quat_regular,quat_flipped;
do {
R = get_random_rotmat();
quat_regular = rotmat2quatwxyz(R);
} while(quat_regular(0)==quat_regular(1) || quat_regular(0)==quat_regular(2) || quat_regular(0)==quat_regular(3) ||
quat_regular(1)==quat_regular(2) || quat_regular(1)==quat_regular(3) ||
quat_regular(2)==quat_regular(3));
// Determine and display the appropriate quaternion trick
quat_flipped = rotmat2quatwxyz(M_oflip*R*M_iflip);
unsigned int item_permuts[4]={0,1,2,3};
int sign_flips[4]={1,1,1,1};
detect_quaternion_trick(quat_regular,quat_flipped,item_permuts,sign_flips);
char str_quat[4]={'w','x','y','z'};
std::cout << std::endl << "When iref is flipped along the " << im << "-axis and oref along the " << om << "-axis:" << std::endl;
std::cout << "resulting_quat=[" << (sign_flips[0]>0?"":"-") << str_quat[item_permuts[0]] << ","
<< (sign_flips[1]>0?"":"-") << str_quat[item_permuts[1]] << ","
<< (sign_flips[2]>0?"":"-") << str_quat[item_permuts[2]] << ","
<< (sign_flips[3]>0?"":"-") << str_quat[item_permuts[3]] << "], where initial_quat=[w,x,y,z]" << std::endl;
// Test this trick on several random rotation matrices
unsigned int n_errors = 0, n_tests = 10000;
std::cout << std::endl << "Performing " << n_tests << " tests on random rotation matrices:" << std::endl;
for(unsigned int i=0; i<n_tests; ++i) {
// Get a random rotation matrix and the corresponding quaternion
cv::Matx<double,3,3> R = get_random_rotmat();
cv::Matx<double,4,1> quat_regular = rotmat2quatwxyz(R);
// Get the quaternion corresponding to the flipped coordinate frames, via the sign trick and via computation on rotation matrices
cv::Matx<double,4,1> quat_tricked = apply_quaternion_trick(item_permuts,sign_flips,quat_regular);
cv::Matx<double,4,1> quat_flipped = rotmat2quatwxyz(M_oflip*R*M_iflip);
// Check that both results are identical
if(cv::norm(quat_tricked-quat_flipped,cv::NORM_INF)>1e-6) {
std::cout << "Error (idx=" << i << ")!"
<< "\n quat_regular=" << quat_regular.t()
<< "\n quat_tricked=" << quat_tricked.t()
<< "\n quat_flipped=" << quat_flipped.t() << std::endl;
++n_errors;
}
}
std::cout << n_errors << " errors on " << n_tests << " tests." << std::endl;
system("pause");
return 0;
}
There is little bit easier and programmer oriented way to think about this. Assume that you want to reverse the z axis (i.e. flip z axis to -z) in your coordinate system. Now think of quaternion as orientation vector in terms of roll, pitch and yaw. When you flip z axis, notice that sign of roll and pitch is inverted but sign for yaw remains same.
Now you can find the net effect on quaternion using following code for converting Euler angles to quaternion (I'd put this code to Wikipedia):
static Quaterniond toQuaternion(double pitch, double roll, double yaw)
{
Quaterniond q;
double t0 = std::cos(yaw * 0.5f);
double t1 = std::sin(yaw * 0.5f);
double t2 = std::cos(roll * 0.5f);
double t3 = std::sin(roll * 0.5f);
double t4 = std::cos(pitch * 0.5f);
double t5 = std::sin(pitch * 0.5f);
q.w() = t0 * t2 * t4 + t1 * t3 * t5;
q.x() = t0 * t3 * t4 - t1 * t2 * t5;
q.y() = t0 * t2 * t5 + t1 * t3 * t4;
q.z() = t1 * t2 * t4 - t0 * t3 * t5;
return q;
}
Using basic trigonometry, sin(-x) = -sin(x) and cos(-x) = cos(x). Applyieng this to above code you can see that sign for t3 and t5 will flip. This will cause sign of x and y to flip.
So when you invert the z-axis,
Q'(w, x, y, z) = Q(w, -x, -y, z)
Similarly you can figure out any other combinations of axis reversal and find impact on quaternion.
PS: In case if anyone is wondering why anyone would ever need this... I needed above to transform quaternion coming from MavLink/Pixhawk system which controls drone. The source system uses NED coordinate system but usual 3D environments like Unreal uses NEU coordinate system which requires transforming z axis to -z to use the quaternion correctly.
I did some further analysis, and it appears the effect of a quaternion (w, x, y, z) can have it's effect mirrored like this:
Mirror effect of rotation along x axis by flipping y and z elements of the quaternion.
Mirror effect of rotation along y axis by flipping x and z elements of the quaternion.
Mirror effect of rotation along z axis by flipping x and y elements of the quaternion.
The w element of the quaternion never needs to be touched.
Unfortunately I still don't understand quaternions well enough to be able to explain why this works, but I derived it from implementations of converting to and from axis-angle format, and after implementing this solution, it works just as well as my original one in all tests of it I have performed.
We can examine the set of all rotations and reflections in 3D this is called the Orthogonal group O(3). It can be though of as the set of orthogonal matrices with determinant +1 or -1. All rotations have determinant +1 and pure reflections have determinate -1. There is another member of O(3) the inversion in a point (x,y,z)->(-x,-y,-z) this has det -1 in 3D and we will come to this later. If we combine two transformations in the group you multiply their determinants. Hence two rotations combined give another rotation (+1 * +1 = +1), a rotation combined with a reflection give a reflection (+1 * -1 = -1) and two reflections combined give a rotation (-1 * -1 = +1).
We can restrict the O(3) to just those with determinant +1 to form the Special Orthogonal Group SO(3). This just contains the rotations.
Now the set of unit quaternions is the double cover of SO(3) that means that two unit quaternions correspond to each rotation. To be precise if a+b i+c j+d k is a unit quaternions then a-b i-c j-d k represents the same rotation, you can think of this as a rotation by ø around the vector (b,c,d) being the same as a rotation by -ø around the vector (-b,-c,-d).
Note that all the unit quaternions have determinant +1, so there is none which correspond to a pure reflection. This is why you cannot use quaternions to represent reflections.
What you might be able to do is use the inversion. Now a reflection followed by an inversion is a rotation. For example reflect in x=0 and invert, is the same as reflecting in the y=0 and reflecting in the z=0. This is the same as 180º rotation around the x-axis. You could do the same procedure for any reflection.
We can define a plane through the origin by using it normal vector n = (a,b,c). A reflection of a vector v(x,y,z) in that plane is given by
v - 2 (v . n ) / ( n . n) n
= (x,y,z) - 2 (a x+b y+c z) / (a^2+b^2+c^2) (a,b,c)
In particular the x-y plane has normal (0,0,1) so a reflection is
(x,y,z) - 2 z (0,0,1) = (x,y,-z)
Quaternions and spatial rotation has a nice formula for a quaternion from the axis angle formula.
p = cos(ø/2) + (x i + y j + z k) sin(ø/2)
This is a quaternion W + X i + Y j + Z k with W=cos(ø/2), X = x sin(ø/2), Y = y sin(ø/2), Z = z sin(ø/2)
Changing the direction of rotation will flip the sin of the half angle but leave the cos unchanged, giving
p' = cos(ø/2) - (x i + y j + z k) sin(ø/2)
Now if we consider reflecting the corresponding vector in x-y plane giving
q = cos(ø/2) + (x i + y j - z k) sin(ø/2)
we might want to change the direction of rotation giving
q' = cos(ø/2) + (- x i - y j + z k) sin(ø/2)
= W - X i - Y j + Z k
which I think corresponds to your answer.
We can generalise this to reflection in a general plane with unit length normal (a,b,c). Let d be the dot product (a,b,c).(x,y,z). The refection of (x,y,z) is
(x,y,z) - 2 d (a,b,c) = (x - 2 d a, y - 2 d b, z - 2 d c)
the rotation quaternion of this
q = cos(ø/2) - ((x - 2 d a) i + ((y - 2 d b) j + (z - 2 d c) k) sin(ø/2)
q = cos(ø/2) - (x i + y j + z k) sin(ø/2)
+ 2 d sin(ø/2) (a i + b j + c k)
= W - X i - Y j - Z k + 2 d (X,Y,Z).(a,b,c) (a i + b j + c k)
Note that mirroring is not a rotation, so generally you can't bake it into a quaternion (I might very well have misunderstood your question, though). The 3x3 component of the mirroring transformation matrix is
M = I-2(n*nT)
where I is an identity 3x3 matrix, n is the mirror plane's normal represented as a 3x1 matrix, and nT is n as a 1x3 matrix (so n*nT is a 3x(1x1)x3=3x3 matrix).
Now, if the quaternion q you want to 'mirror' is the last transformation, the last transformation on the other side would be just M*q (again, this would be a general 3x3 matrix, not generally representable as a quaternion)
For anyone who gets here by a web-search and is looking for the math, then:
Reflection
To reflecting point 'p' through plane ax+by+cz=0, using quaternions:
n = 0+(a,b,c)
p = 0+(x,y,z)
where 'n' is a unit bivector (or pure quaternion if you prefer)
p' = npn
then p' is the reflect point.
If you compose with a second reflection 'm':
p' = mnpnm = (mn)p(mn)^*
is a rotation.
Rotations and reflections compose as expected.
Uniform scaling
Since scalar products commute and can be factor out then if we have either a rotation by unit quaternion 'Q' or a reflection by unit bivector 'b' (or any combination of) multiplying either by some non-zero scale value 's' results in a uniform scaling of s^2. And since (sqrt(s0)*sqrt(s1))^2 = s0*s1, these uniform scaling value compose as expected.
However this point is probably of no interest since in code we want to be able to assume unit magnitude values to reduce the runtime complexity.
Let's say I have an entity in 3d space and I know its position vector (x, y, z) and its velocity vector (so its orientation in space).
Starting from known point A I want to reach known point B in two steps:
a) turn, following a circular path with a known radius R, until point C
b) go straight from point C to final point B.
Speed (scalar value) is not important. Velocity (vector) is known, so I guess it should define the plane on which the circle resides, being tangent to it, together with the line between A and B...
I want to know how to find coordinates (x, y, z) for C and the velocity vector of the entity being there.
Update: see the working demo!
If I understand correctly, your situation (in the plane containing A, B and v) is as shown in the diagram below. The points A and B are given, as is the vector v and the distance r. You want to find the point C.
Well, let the vector w = (−v̂y, v̂x) be a unit vector perpendicular to v. Then O = A + r w.
Now, |C − O| = r and (C − B)·(C − O) = 0 (where · is the dot product). Combine these to get a quadratic equation, which you can solve to find the two possible positions for C. Then pick the one with the right sign for (C − B)×(C − O).
(There's a second choice for the centre of the circle, O = A − r w, representing turning clockwise instead of anticlockwise. This gives you another possibility for C. I guess you'll have to use some heuristic to decide which one you prefer: maybe the one with smallest ∠AOC.)
St0rM asks for help with doing this in 3D (see comments). That's easy! The plane containing A, B, and v has normal vector n = (A − B) × v. Let u = n × v be a vector perpendicular to both n and v, and let w = û (the unit vector in the direction of u).
You'll also need to take into account the constraint that C lies in the same plane as A: C·n = A.n, and "the right sign for (C − B)×(C − O)" becomes "the right sign for (C − B)×(C − O)·n".
Having trouble solving this system of equations?
Well, if (C − B)·(C − O) = 0, then (C − O + O − B)·(C − O) = 0, therefore (C − O)·(C − O) + (O − B)·(C − O) = 0, therefore C·(O − B) = O·(O − B) − r2.
You'll note that this is the equation for a plane, and so is C·n = A.n. Intersect these two planes (see Wikipedia for details — you can use the simpler solution since the planes are orthogonal and can easily be made orthonormal) to get the equation of a line on which C lies: C = H + λL, say, where L = n×(B − O). Then use (C − O)·(C − O) = r2 to turn this into a quadratic equation in λ. You'll find that the quadratic equation simplifies quite a bit if you rewrite the equation of the line as C = H + λL + O so that occurrences of "− O" disappear.
Here's an implementation in Python using numpy to do the vector algebra. I'm sure you can figure out how to convert this to the language of your choice.
import math
from numpy import cross, dot
from numpy.linalg import norm
def unit(v):
"""Return a unit vector in the same direction as v."""
return v / norm(v)
def turnpoints(A, B, v, r):
"""Generate possible turning instructions for a path from A to B
that starts out in direction v, turns through part of a circle of radius
r until it reaches a point C (to be determined), then heads straight for
B. Return each instruction in the form (sense, C) where sense is -1 for
clockwise and +1 for anticlockwise."""
n = unit(cross(A - B, v)) # Unit normal to plane containing A, B, v.
w = unit(cross(n, v)) # Unit normal to v lying in that plane.
for sense in (-1, +1): # Turn clockwise or anticlockwise?
O = A + sense * r * w # Centre of turning circle.
BB = B - O
m = unit(BB)
# C lies on the line H + l*L + O
H = dot(A, n) * n + (r**2 / norm(BB)) * m
L = cross(n, m)
# |C - O| = r**2 gives quadratic eqn in l with coefficients a=1, b=0, c.
c = dot(H, H) - r**2
disc = - 4 * c # Discriminant of quadratic eqn.
if disc < 0:
continue # No tangents (B inside circle).
elif disc == 0: # One tangent (B on circle).
C = H + O
yield (sense, C)
else: # Two tangents (B outside circle)
for sign in (-1, +1):
l = sign * math.sqrt(disc) / 2
C = H + l * L + O
# Only one choice for C is correct (the other involves
# reversing direction).
if dot(cross(C - B, C - O), n) * sense > 0:
yield (sense, C)
Woo, this is interesting.
Find the plane that points A, B, and the entity's velocity exist on. Everything will happen on this plane, so now you can think of the problem as 2D, though the math will still be 3D.
Find the center point of the circle the entity will be traveling along on the plane. This center point will be perpendicular to the entity's velocity, at a distance of R. There will actually be 2 different places you could place the circle, you'll need to choose based on which is closer to B.
Find the two lines from B that are tangential to the circle. The points where these lines touch the circle are your two possibilities for C. Determine which point will allow your entity to hit B when it reaches it and leaves the circle (the other point will bring the entity in exactly the opposite direction).
From this, you should be able to get the position of C, and I'm fairly certain that, if the entity doesn't change speed, the vector should just be the unit vector from C to B times the magnitude of the original velocity vector.
Any or all of this could be wrong. I'm just figuring this from doodling on a piece of paper.
Perhaps this is a shorter version. You could compute C from:
H = O + (r^2)/|BB|)*m
C = H + l*(sqrt(r^2 - |HO|^2)) //this simply applies Pytagora to find out HC magnitude
as a proof of concept:
cml::vector3f A( 4, 3, 2);
cml::vector3f B( 1, 5, 0);
cml::vector3f v( 1, 0, 0);
float r = 1.4142;
cml::vector3f n = cml::cross((A-B),v);
n.normalize();
cout << "n: " << n << endl;
cml::vector3f w = cml::cross(n,v);
w.normalize();
cout << "w: " << w << endl;
cml::vector3f O = A + r*w;
cout << "O: " << O << endl;
cml::vector3f m = B-O;
m.normalize();
cout << "m: " << m << endl;
cml::vector3f OB = B - O;
cml::vector3f H = O + m * ( pow(r,2) / OB.length() );
cout << "H: " << H << endl;
cml::vector3f l = cml::cross(m,n);;
l.normalize();
cout << "l: " << l << endl;
cml::vector3f OH = H - O;
cml::vector3f C = H + l * ( sqrt( pow(r,2)-pow(OH.length(),2) ) );
cout << "C: " << C << endl;
There is a very handy set of 2d geometry utilities here.
The angleBetweenLines has a problem, though. The result is always positive. I need to detect both positive and negative angles, so if one line is 15 degrees "above" or "below" the other line, the shape obviously looks different.
The configuration I have is that one line remains stationary, while the other line rotates, and I need to understand what direction it is rotating in, by comparing it with the stationary line.
EDIT: in response to swestrup's comment below, the situation is actually that I have a single line, and I record its starting position. The line then rotates from its starting position, and I need to calculate the angle from its starting position to current position. E.g if it has rotated clockwise, it is positive rotation; if counterclockwise, then negative. (Or vice versa.)
How to improve the algorithm so it returns the angle as both positive or negative depending on how the lines are positioned?
Here's the implementation of brainjam's suggestion. (It works with my constraints that the difference between the lines is guaranteed to be small enough that there's no need to normalize anything.)
CGFloat angleBetweenLinesInRad(CGPoint line1Start, CGPoint line1End, CGPoint line2Start, CGPoint line2End) {
CGFloat a = line1End.x - line1Start.x;
CGFloat b = line1End.y - line1Start.y;
CGFloat c = line2End.x - line2Start.x;
CGFloat d = line2End.y - line2Start.y;
CGFloat atanA = atan2(a, b);
CGFloat atanB = atan2(c, d);
return atanA - atanB;
}
I like that it's concise. Would the vector version be more concise?
#duffymo's answer is correct, but if you don't want to implement cross-product, you can use the atan2 function. This returns an angle between -π and π, and you can use it on each of the lines (or more precisely the vectors representing the lines).
If you get an angle θ for the first (stationary line), you'll have to normalize the angle φ for the second line to be between θ-π and θ+π (by adding ±2π). The angle between the two lines will then be φ-θ.
This is an easy problem involving 2D vectors. The sine of the angle between two vectors is related to the cross-product between the two vectors. And "above" or "below" is determined by the sign of the vector that's produced by the cross-product: if you cross two vectors A and B, and the cross-product produced is positive, then A is "below" B; if it's negative, A is "above" B. See Mathworld for details.
Here's how I might code it in Java:
package cruft;
import java.text.DecimalFormat;
import java.text.NumberFormat;
/**
* VectorUtils
* User: Michael
* Date: Apr 18, 2010
* Time: 4:12:45 PM
*/
public class VectorUtils
{
private static final int DEFAULT_DIMENSIONS = 3;
private static final NumberFormat DEFAULT_FORMAT = new DecimalFormat("0.###");
public static void main(String[] args)
{
double [] a = { 1.0, 0.0, 0.0 };
double [] b = { 0.0, 1.0, 0.0 };
double [] c = VectorUtils.crossProduct(a, b);
System.out.println(VectorUtils.toString(c));
}
public static double [] crossProduct(double [] a, double [] b)
{
assert ((a != null) && (a.length >= DEFAULT_DIMENSIONS ) && (b != null) && (b.length >= DEFAULT_DIMENSIONS));
double [] c = new double[DEFAULT_DIMENSIONS];
c[0] = +a[1]*b[2] - a[2]*b[1];
c[1] = +a[2]*b[0] - a[0]*b[2];
c[2] = +a[0]*b[1] - a[1]*b[0];
return c;
}
public static String toString(double [] a)
{
StringBuilder builder = new StringBuilder(128);
builder.append("{ ");
for (double c : a)
{
builder.append(DEFAULT_FORMAT.format(c)).append(' ');
}
builder.append("}");
return builder.toString();
}
}
Check the sign of the 3rd component. If it's positive, A is "below" B; if it's negative, A is "above" B - as long as the two vectors are in the two quadrants to the right of the y-axis. Obviously, if they're both in the two quadrants to the left of the y-axis the reverse is true.
You need to think about your intuitive notions of "above" and "below". What if A is in the first quadrant (0 <= θ <= 90) and B is in the second quadrant (90 <= θ <= 180)? "Above" and "below" lose their meaning.
The line then rotates from its
starting position, and I need to
calculate the angle from its starting
position to current position. E.g if
it has rotated clockwise, it is
positive rotation; if
counterclockwise, then negative. (Or
vice versa.)
This is exactly what the cross-product is for. The sign of the 3rd component is positive for counter-clockwise and negative for clockwise (as you look down at the plane of rotation).
One 'quick and dirty' method you can use is to introduce a third reference line R. So, given two lines A and B, calculate the angles between A and R and then B and R, and subtract them.
This does about twice as much calculation as is actually necessary, but is easy to explain and debug.
// Considering two vectors CA and BA
// Computing angle from CA to BA
// Thanks to code shared by Jaanus, but atan2(y,x) is used wrongly.
float getAngleBetweenVectorsWithSignInDeg(Point2f C, Point2f A, Point2f B)
{
float a = A.x - C.x;
float b = A.y - C.y;
float c = B.x - C.x;
float d = B.y - C.y;
float angleA = atan2(b, a);
float angleB = atan2(d, c);
cout << "angleA: " << angleA << "rad, " << angleA * 180 / M_PI << " deg" << endl;
cout << "angleB: " << angleB << "rad, " << angleB * 180 / M_PI << " deg" << endl;
float rotationAngleRad = angleB - angleA;
float thetaDeg = rotationAngleRad * 180.0f / M_PI;
return thetaDeg;
}
That function is working in RADS
There are 2pi RADS in a full circle (360 degrees)
Thus I believe the answear you are looking for is simply the returned value - 2pi
If you are asking to have that one function return both values at the same time, then you are asking to break the language, a function can only return a single value. You could pass it two pointers that it can use to set the value of so that the change can persist after the frunction ends and your program can continue to work. But not really a sensible way of solving this problem.
Edit
Just noticed that the function actually converts the Rads to Degrees as it returns the value. But the same principle will work.