I have a sprite that moves along a vector (-0.7,-0.3). I have another point whose coordinates I have - let's call them (xB|yB). Now, quite some time ago I learned to calculate the perpendicular distance from a vector to a point (first formula on this page http://en.wikipedia.org/wiki/Perpendicular_distance). However I tried it, and if I log it, it returns an unbelievably high value that is 100% false. So what do I do wrong ? Have a look at the image I provided.
incomingVector = (-0.7,-0.3) //this is the vector the sprite is moving along
bh.position is the point I want to calculate the distance to
Here is the code:
// first I am working out the c Value in the formula in the link given above
CGPoint pointFromVector = CGPointMake(bh.incomingVector.x*theSprite.position.x,bh.incomingVector.y*theSprite.position.y);
float result = pointFromVector.x + pointFromVector.y;
float result2 = (-1)*result;
//now I use the formula
float test = (bh.incomingVector.x * bh.position.x + bh.incomingVector.y * bh.position.y + result2)/sqrt(pow(bh.incomingVector.x, 2)+pow(bh.incomingVector.y, 2));
//the distance has to be positive, so I do the following
if(test < 0){
test *= (-1);
}
let us implement the formula again, according to the contents of your original link.
we have a vector for the line: V(a; b)
we have a point on the line (the centre of the sprite): P(x1, y1)
we have another point somewhere else: B(xB, yB)
for the testing here are two rows of random values:
a = -0.7; b = -0.3; x1 = 7; y1 = 7; xB = 5; yB = 5;
a = -0.7; b = -0.3; x1 = 7; y1 = 7; xB = 5.5; yB = 4;
the numerator is the following then: (it seems you are calculating the numerator an unknown way, I don't understand why you did it because this is the proper way to calculate the numerator for the linked formula, perhaps this is why you got totally wrong distances.)
float _numerator = abs((b * xB) - (a * yB) - (b * x1) + (a * y1));
// for the 1. test values: (-0.3 * 5) - (-0.7 * 5) - (-0.3 * 7) + (-0.7 * 7) = -1.5 + 3.5 + 2.1 - 4.9 = -0.8 => abs(-0.8) = 0.8
// for the 2. test values: (-0.3 * 5.5) - (-0.7 * 4) - (-0.3 * 7) + (-0.7 * 7) = -1.65 + 2.8 + 2.1 - 4.9 = -1.65 => abs(-1.65) = 1.65
the denominator is the following then:
float _denomimator = sqrt((a * a) + (b * b));
// for the 1. test values: (-0.7 * -0.7) + (-0.3 * -0.3) = 0.49 + 0.09 = 0.58 => sort(0.58) = 0.76
// for the 2. test values: (-0.7 * -0.7) + (-0.3 * -0.3) = 0.49 + 0.09 = 0.58 => sort(0.58) = 0.76
the distance is obvious now:
float _distance = _numerator / _denominator;
// for the 1. test values: 0.8 / 0.76 = 1.05
// for the 2. test values: 1.65 / 0.76 = 2.17
and these results (1.05 and 2.17) are the correct distances exactly for our random values, if you can draw the lines and the points on the paper you can measure the distance and you would get the same values, using standard ruler.
Related
I have a camera (in a custom 3D engine) that accepts a quaternion for the rotation transform. I have two 3D points representing a camera and an object to look at. I want to calculate the quaternion that looks from the camera to the object, while respecting the world up axis.
This question asks for the same thing without the "up" vector. All three answers result in the camera pointing in the correct direction, but rolling (as in yaw/pitch/roll; imagine leaning your head onto your ear while looking at something).
I can calculate an orthonormal basis of vectors that match the desired coordinate system by:
lookAt = normalize(target - camera)
sideaxis = cross(lookAt, worldUp)
rotatedup = cross(sideaxis, lookAt)
How can I create a quaternion from those three vectors? This question asks for the same thing...but unfortunately the only and accepted answer says ~"let's assume you don't care about roll", and then goes about ignoring the up axis. I do care about roll. I don't want to ignore the up axis.
A previous answer has given a valid solution using angles. This answer will present an alternative method.
The orthonormal basis vectors, renaming them F = lookAt, R = sideaxis, U = rotatedup, directly form the columns of the 3x3 rotation matrix which is equivalent to your desired quaternion:
Multiplication with a vector is equivalent to using said vector's components as the coordinates in the camera's basis.
A 3x3 rotation matrix can be converted into a quaternion without conversion to angles / use of costly trigonometric functions. Below is a numerically stable C++ snippet which does this, returning a normalized quaternion:
inline void CalculateRotation( Quaternion& q ) const {
float trace = a[0][0] + a[1][1] + a[2][2];
if( trace > 0 ) {
float s = 0.5f / sqrtf(trace + 1.0f);
q.w = 0.25f / s;
q.x = ( a[2][1] - a[1][2] ) * s;
q.y = ( a[0][2] - a[2][0] ) * s;
q.z = ( a[1][0] - a[0][1] ) * s;
} else {
if ( a[0][0] > a[1][1] && a[0][0] > a[2][2] ) {
float s = 2.0f * sqrtf( 1.0f + a[0][0] - a[1][1] - a[2][2]);
q.w = (a[2][1] - a[1][2] ) / s;
q.x = 0.25f * s;
q.y = (a[0][1] + a[1][0] ) / s;
q.z = (a[0][2] + a[2][0] ) / s;
} else if (a[1][1] > a[2][2]) {
float s = 2.0f * sqrtf( 1.0f + a[1][1] - a[0][0] - a[2][2]);
q.w = (a[0][2] - a[2][0] ) / s;
q.x = (a[0][1] + a[1][0] ) / s;
q.y = 0.25f * s;
q.z = (a[1][2] + a[2][1] ) / s;
} else {
float s = 2.0f * sqrtf( 1.0f + a[2][2] - a[0][0] - a[1][1] );
q.w = (a[1][0] - a[0][1] ) / s;
q.x = (a[0][2] + a[2][0] ) / s;
q.y = (a[1][2] + a[2][1] ) / s;
q.z = 0.25f * s;
}
}
}
Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion
Converting this to suit your situation is of course just a matter of swapping the matrix elements with the corresponding vector components:
// your code from before
F = normalize(target - camera); // lookAt
R = normalize(cross(F, worldUp)); // sideaxis
U = cross(R, F); // rotatedup
// note that R needed to be re-normalized
// since F and worldUp are not necessary perpendicular
// so must remove the sin(angle) factor of the cross-product
// same not true for U because dot(R, F) = 0
// adapted source
Quaternion q;
double trace = R.x + U.y + F.z;
if (trace > 0.0) {
double s = 0.5 / sqrt(trace + 1.0);
q.w = 0.25 / s;
q.x = (U.z - F.y) * s;
q.y = (F.x - R.z) * s;
q.z = (R.y - U.x) * s;
} else {
if (R.x > U.y && R.x > F.z) {
double s = 2.0 * sqrt(1.0 + R.x - U.y - F.z);
q.w = (U.z - F.y) / s;
q.x = 0.25 * s;
q.y = (U.x + R.y) / s;
q.z = (F.x + R.z) / s;
} else if (U.y > F.z) {
double s = 2.0 * sqrt(1.0 + U.y - R.x - F.z);
q.w = (F.x - R.z) / s;
q.x = (U.x + R.y) / s;
q.y = 0.25 * s;
q.z = (F.y + U.z) / s;
} else {
double s = 2.0 * sqrt(1.0 + F.z - R.x - U.y);
q.w = (R.y - U.x) / s;
q.x = (F.x + R.z) / s;
q.y = (F.y + U.z) / s;
q.z = 0.25 * s;
}
}
(And needless to say swap y and z if you're using OpenGL.)
Assume you initially have three ortonormal vectors: worldUp, worldFront and worldSide, and lets use your equations for lookAt, sideAxis and rotatedUp. The worldSide vector will not be necessary to achieve the result.
Break the operation in two. First, rotate around worldUp. Then rotate around sideAxis, which will now actually be parallel to the rotated worldSide.
Axis1 = worldUp
Angle1 = (see below)
Axis2 = cross(lookAt, worldUp) = sideAxis
Angle2 = (see below)
Each of these rotations correspond to a quaternion using:
Q = cos(Angle/2) + i * Axis_x * sin(Angle/2) + j * Axis_y * sin(Angle/2) + k * Axis_z * sin(Angle/2)
Multiply both Q1 and Q2 and you get the desired quaternion.
Details for the angles:
Let P(worldUp) be the projection matrix on the worldUp direction, i.e., P(worldUp).v = cos(worldUp,v).worldUp or using kets and bras, P(worldUp) = |worldUp >< worldUp|. Let I be the identity matrix.
Project lookAt in the plane perpendicular to worldUp and normalize it.
tmp1 = (I - P(worldUp)).lookAt
n1 = normalize(tmp1)
Angle1 = arccos(dot(worldFront,n1))
Angle2 = arccos(dot(lookAt,n1))
EDIT1:
Notice that there is no need to compute transcendental functions. Since the dot product of a pair of normalized vectors is the cosine of an angle and assuming that cos(t) = x, we have the trigonometric identities:
cos(t/2) = sqrt((1 + x)/2)
sin(t/2) = sqrt((1 - x)/2)
If somebody search for C# version with handling every matrix edge cases (not input edge cases!), here it is:
public static SoftQuaternion LookRotation(SoftVector3 forward, SoftVector3 up)
{
forward = SoftVector3.Normalize(forward);
// First matrix column
SoftVector3 sideAxis = SoftVector3.Normalize(SoftVector3.Cross(up, forward));
// Second matrix column
SoftVector3 rotatedUp = SoftVector3.Cross(forward, sideAxis);
// Third matrix column
SoftVector3 lookAt = forward;
// Sums of matrix main diagonal elements
SoftFloat trace1 = SoftFloat.One + sideAxis.X - rotatedUp.Y - lookAt.Z;
SoftFloat trace2 = SoftFloat.One - sideAxis.X + rotatedUp.Y - lookAt.Z;
SoftFloat trace3 = SoftFloat.One - sideAxis.X - rotatedUp.Y + lookAt.Z;
// If orthonormal vectors forms identity matrix, then return identity rotation
if (trace1 + trace2 + trace3 < SoftMath.CalculationsEpsilon)
{
return Identity;
}
// Choose largest diagonal
if (trace1 + SoftMath.CalculationsEpsilon > trace2 && trace1 + SoftMath.CalculationsEpsilon > trace3)
{
SoftFloat s = SoftMath.Sqrt(trace1) * (SoftFloat)2.0f;
return new SoftQuaternion(
(SoftFloat)0.25f * s,
(rotatedUp.X + sideAxis.Y) / s,
(lookAt.X + sideAxis.Z) / s,
(rotatedUp.Z - lookAt.Y) / s);
}
else if (trace2 + SoftMath.CalculationsEpsilon > trace1 && trace2 + SoftMath.CalculationsEpsilon > trace3)
{
SoftFloat s = SoftMath.Sqrt(trace2) * (SoftFloat)2.0f;
return new SoftQuaternion(
(rotatedUp.X + sideAxis.Y) / s,
(SoftFloat)0.25f * s,
(lookAt.Y + rotatedUp.Z) / s,
(lookAt.X - sideAxis.Z) / s);
}
else
{
SoftFloat s = SoftMath.Sqrt(trace3) * (SoftFloat)2.0f;
return new SoftQuaternion(
(lookAt.X + sideAxis.Z) / s,
(lookAt.Y + rotatedUp.Z) / s,
(SoftFloat)0.25f * s,
(sideAxis.Y - rotatedUp.X) / s);
}
}
This realization based on deeper understanding of this conversation, and was tested for many edge case scenarios.
P.S.
Quaternion's constructor is (x, y, z, w)
SoftFloat is software float type, so you can easyly change it to built-in float if needed
For full edge case safe realization (including input) check this repo.
lookAt
sideaxis
rotatedup
If you normalize this 3 vectors, it is a components of rotation matrix 3x3. So just convert this rotation matrix to quaternion.
Im trying to understand what this function does. It was given by my teacher and I just cant understands, whats logic behind the formulas finding x, and y coordinates. From my math class I know I my formulas for finding interception but its confusing translated in code. So I have some problems how they defined the formulas for a,b,c and for finding the coordinates x and y.
void Intersection::getIntersectionPoints(const Arc& arc, const Line& line) {
double a, b, c, mu, det;
std::pair<double, double> xPoints;
std::pair<double, double> yPoints;
std::pair<double, double> zPoints;
//(m2+1)x2+2(mc−mq−p)x+(q2−r2+p2−2cq+c2)=0.
//a= m2;
//b= 2 * (mc - mq - p);
//c= q2−r2+p2−2cq+c2
a = pow((line.end().x - line.start().x), 2) + pow((line.end().y - line.start().y), 2) + pow((line.end().z - line.start().z), 2);
b = 2 * ((line.end().x - line.start().x)*(line.start().x - arc.center().x)
+ (line.end().y - line.start().y)*(line.start().y - arc.center().y)
+ (line.end().z - line.start().z)*(line.start().z - arc.center().z));
c = pow((arc.center().x), 2) + pow((arc.center().y), 2) +
pow((arc.center().z), 2) + pow((line.start().x), 2) +
pow((line.start().y), 2) + pow((line.start().z), 2) -
2 * (arc.center().x * line.start().x + arc.center().y * line.start().y +
arc.center().z * line.start().z) - pow((arc.radius()), 2);
det = pow(b, 2) - 4 * a * c;
/* Tangenta na kružnicu */
if (Math<double>::isEqual(det, 0.0, 0.00001)) {
if (!Math<double>::isEqual(a, 0.0, 0.00001))
mu = -b / (2 * a);
else
mu = 0.0;
// x = h + t * ( p − h )
xPoints.second = xPoints.first = line.start().x + mu * (line.end().x - line.start().x);
yPoints.second = yPoints.first = line.start().y + mu * (line.end().y - line.start().y);
zPoints.second = zPoints.first = line.start().z + mu * (line.end().z - line.start().z);
}
if (Math<double>::isGreater(det, 0.0, 0.00001)) {
// first intersection
mu = (-b - sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
xPoints.first = line.start().x + mu * (line.end().x - line.start().x);
yPoints.first = line.start().y + mu * (line.end().y - line.start().y);
zPoints.first = line.start().z + mu * (line.end().z - line.start().z);
// second intersection
mu = (-b + sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
xPoints.second = line.start().x + mu * (line.end().x - line.start().x);
yPoints.second = line.start().y + mu * (line.end().y - line.start().y);
zPoints.second = line.start().z + mu * (line.end().z - line.start().z);
}
Denoting the line's start point as A, end point as B, circle's center as C, circle's radius as r and the intersection point as P, then we can write P as
P=(1-t)*A + t*B = A+t*(B-A) (1)
Point P will also locate on the circle, therefore
|P-C|^2 = r^2 (2)
Plugging equation (1) into equation (2), you will get
|B-A|^2*t^2 + 2(B-A)\dot(A-C)*t +(|A-C|^2 - r^2) = 0 (3)
This is how you get the formula for a, b and c in the program you posted. After solving for t, you shall obtain the intersection point(s) from equation (1). Since equation (3) is quadratic, you might get 0, 1 or 2 values for t, which correspond to the geometric configurations where the line might not intersect the circle, be exactly tangent to the circle or pass thru the circle at two locations.
I am using Goerzel to id a certain frequency .
What i see is that it works great-but in a strange way- when i input to it samples(±500/1024) i get the right values-but they becomes lower and lower till zero -while the frequency is STILL there . so i get for ex: 700, than it goes slowly down ..
Also, i would like to make it more exponential -so differences between noise and frequency will be higher .
What can cause this problem ,and how can i improve my code ?
thanks.
float goertzel_mag(int16_t* data ,int SAMPLING_RATE ,double TARGET_FREQUENCY,int numSamples )
{
int k,i;
float floatnumSamples;
float omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
float scalingFactor = numSamples / 2.0; // -2
floatnumSamples = (float) numSamples;
k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
omega = (2.0 * M_PI * k) / floatnumSamples;
sine = sin(omega);
cosine = cos(omega);
coeff = 2.0 * cosine;
q0=0;
q1=0;
q2=0;
for(i=0; i<numSamples; i++)
{
q0 = coeff * q1 - q2 + data[i];
q2 = q1;
q1 = q0;
}
real = (q1 - q2 * cosine) / scalingFactor;
imag = (q2 * sine) / scalingFactor;
//double theta = atan2 ( imag, real); //PHASE
magnitude = sqrtf(real*real + imag*imag);
return magnitude;
}
After SO much researches about Goerzel , i found out that the problem is not him .
When i input a pure sin wave to the mac , and print out the buffer :
int16_t *q = (int16_t *)(&bufferList)->mBuffers[0].mData;
Its values are becomes high, but after 5 seconds- the signal is going lower and lower to zero!
Moving the signal source, will make it again becomes higher, and goes down again.
For what i have read , the chanel can go into saturation , and maybe this can cause the problem.
This Goerzel algorithm is very good .
I have a space ship that I want to turn to a destination angle. Currently it works like 90% of the time, but sometimes, it 'jumps' to the destination angle rather than moving smoothly. Here is my code:
a = System.Math.Sin(.destStoppingAngle + System.Math.PI)
b = System.Math.Cos(.destStoppingAngle + System.Math.PI)
c = System.Math.Sin(.msngFacing)
d = System.Math.Cos(.msngFacing)
det = a * d - b * c
If det > 0 Then
.msngFacing = .msngFacing - .ROTATION_RATE * TV.TimeElapsed
If det < 0.1 Then
.msngFacing = .destStoppingAngle
.turning = False
End If
Else
.msngFacing = .msngFacing + .ROTATION_RATE * TV.TimeElapsed
If det > 0.1 Then
.msngFacing = .destStoppingAngle
.turning = False
End If
End If
I would do it like this. First you need a function to lerp an angle (C code, port it yourself):
float lerpangle(float from, float to, float frac) {
float a;
if ( to - from > 180 ) {
to -= 360;
}
if ( to - from < -180 ) {
to += 360;
}
a = from + frac * (to - from);
return a;
}
Then, when starting the rotation you have the duration and stoppingangle as your own parameters. Get the startingangle from your object and startingtime (in something decently precise, milliseconds) and save them. The rotation then goes like this:
current_rotation = lerpangle(startingangle, stoppingangle,
(time.now - startingtime) / duration)
I need to write a program to generate and display a piecewise quadratic Bezier curve that interpolates each set of data points (I have a txt file contains data points). The curve should have continuous tangent directions, the tangent direction at each data point being a convex combination of the two adjacent chord directions.
0.1 0,
0 0,
0 5,
0.25 5,
0.25 0,
5 0,
5 5,
10 5,
10 0,
9.5 0
The above are the data points I have, does anyone know what formula I can use to calculate control points?
You will need to go with a cubic Bezier to nicely handle multiple slope changes such as occurs in your data set. With quadratic Beziers there is only one control point between data points and so each curve segment much be all on one side of the connecting line segment.
Hard to explain, so here's a quick sketch of your data (black points) and quadratic control points (red) and the curve (blue). (Pretend the curve is smooth!)
Look into Cubic Hermite curves for a general solution.
From here: http://blog.mackerron.com/2011/01/01/javascript-cubic-splines/
To produce interpolated curves like these:
You can use this coffee-script class (which compiles to javascript)
class MonotonicCubicSpline
# by George MacKerron, mackerron.com
# adapted from:
# http://sourceforge.net/mailarchive/forum.php?thread_name=
# EC90C5C6-C982-4F49-8D46-A64F270C5247%40gmail.com&forum_name=matplotlib-users
# (easier to read at http://old.nabble.com/%22Piecewise-Cubic-Hermite-Interpolating-
# Polynomial%22-in-python-td25204843.html)
# with help from:
# F N Fritsch & R E Carlson (1980) 'Monotone Piecewise Cubic Interpolation',
# SIAM Journal of Numerical Analysis 17(2), 238 - 246.
# http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
# http://en.wikipedia.org/wiki/Cubic_Hermite_spline
constructor: (x, y) ->
n = x.length
delta = []; m = []; alpha = []; beta = []; dist = []; tau = []
for i in [0...(n - 1)]
delta[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i])
m[i] = (delta[i - 1] + delta[i]) / 2 if i > 0
m[0] = delta[0]
m[n - 1] = delta[n - 2]
to_fix = []
for i in [0...(n - 1)]
to_fix.push(i) if delta[i] == 0
for i in to_fix
m[i] = m[i + 1] = 0
for i in [0...(n - 1)]
alpha[i] = m[i] / delta[i]
beta[i] = m[i + 1] / delta[i]
dist[i] = Math.pow(alpha[i], 2) + Math.pow(beta[i], 2)
tau[i] = 3 / Math.sqrt(dist[i])
to_fix = []
for i in [0...(n - 1)]
to_fix.push(i) if dist[i] > 9
for i in to_fix
m[i] = tau[i] * alpha[i] * delta[i]
m[i + 1] = tau[i] * beta[i] * delta[i]
#x = x[0...n] # copy
#y = y[0...n] # copy
#m = m
interpolate: (x) ->
for i in [(#x.length - 2)..0]
break if #x[i] <= x
h = #x[i + 1] - #x[i]
t = (x - #x[i]) / h
t2 = Math.pow(t, 2)
t3 = Math.pow(t, 3)
h00 = 2 * t3 - 3 * t2 + 1
h10 = t3 - 2 * t2 + t
h01 = -2 * t3 + 3 * t2
h11 = t3 - t2
y = h00 * #y[i] +
h10 * h * #m[i] +
h01 * #y[i + 1] +
h11 * h * #m[i + 1]
y