I have a quaternion Qa and Qb.
I rotate Qa by Qb to get Qc, by multiplication:
Qa = {x, y, z, w}
Qb = {x, y, z, w}
Qc = {
x = Qa.w*Qb.x + Qa.x*Qb.w + Qa.y*Qb.z - Qa.z*Qb.y,
y = Qa.w*Qb.y + Qa.y*Qb.w + Qa.z*Qb.x - Qa.x*Qb.z,
z = Qa.w*Qb.z + Qa.z*Qb.w + Qa.x*Qb.y - Qa.y*Qb.x,
w = Qa.w*Qb.w - Qa.x*Qb.x - Qa.y*Qb.y - Qa.z*Qb.z,
}
How can I retrieve Qb again from Qc and Qa?
Feel free to edit the title I'm not sure what to call it.
I tried to multiply Qc with inverse Qa, but it's the wrong approach.
This question might be suited for the math stack but i'm looking at how to implement it in code.
If Qc = Qa * Qb you get Qb by Qb = QaInv * Qc where QaInv is the inverse of Qa. Keep in mind that the order of the two quaternions must be as given. Changing the order of both operands will in most cases lead to a wrong result.
Related
As shown in the image below, I'm creating a program that will make a 2D animation of a truck that is made up of two articulated parts.
The truck pulls the trailer.
The trailer moves according to the docking axis on the truck.
Then, when the truck turns, the trailer should gradually align itself with the new angle of the truck, as it does in real life.
I would like to know if there is any formula or algorithm that does this calculation in an easy way.
I've already seen inverse kinematics equations, but I think for just 2 parts it would not be so complex.
Can anybody help me?
Let A be the midpoint under the front axle, B be the midpoint under the middle axle, and C be the midpoint under the rear axle. For simplicity assume that the hitch is at point B. These are all functions of time t, for example A(t) = (a_x(t), a_y(t).
The trick is this. B is moving directly towards A with the component of A's velocity in that direction. Or in symbols, dB/dt = (dA/dt).(A-B)/||A-B|| And similarly, dC/dt = (dB/dt).(B-C)/||B-C|| where . is the dot product.
This turns into a non-linear first-order system in 6 variables. This can be solved with normal techniques, such as https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods.
UPDATE: Added code
Here is a Python implementation. You can replace it with https://rosettacode.org/wiki/Runge-Kutta_method for your favorite language and your favorite linear algebra library. Or even hand-roll that.
For my example I started with A at (1, 1), B at (2, 1) and C at (2, 2). Then pulled A to the origin in steps of size 0.01. That can be altered to anything that you want.
#! /usr/bin/env python
import numpy
# Runga Kutta method.
def RK4(f):
return lambda t, y, dt: (
lambda dy1: (
lambda dy2: (
lambda dy3: (
lambda dy4: (dy1 + 2*dy2 + 2*dy3 + dy4)/6
)( dt * f( t + dt , y + dy3 ) )
)( dt * f( t + dt/2, y + dy2/2 ) )
)( dt * f( t + dt/2, y + dy1/2 ) )
)( dt * f( t , y ) )
# da is a function giving velocity of a at a time t.
# The other three are the positions of the three points.
def calculate_dy (da, A0, B0, C0):
l_ab = float(numpy.linalg.norm(A0 - B0))
l_bc = float(numpy.linalg.norm(B0 - C0))
# t is time, y = [A, B, C]
def update (t, y):
(A, B, C) = y
dA = da(t)
ab_unit = (A - B) / float(numpy.linalg.norm(A-B))
# The first term is the force. The second is a correction to
# cause roundoff errors in length to be selfcorrecting.
dB = (dA.dot(ab_unit) + float(numpy.linalg.norm(A-B))/l_ab - l_ab) * ab_unit
bc_unit = (B - C) / float(numpy.linalg.norm(B-C))
# The first term is the force. The second is a correction to
# cause roundoff errors in length to be selfcorrecting.
dC = (dB.dot(bc_unit) + float(numpy.linalg.norm(B-C))/l_bc - l_bc) * bc_unit
return numpy.array([dA, dB, dC])
return RK4(update)
A0 = numpy.array([1.0, 1.0])
B0 = numpy.array([2.0, 1.0])
C0 = numpy.array([2.0, 2.0])
dy = calculate_dy(lambda t: numpy.array([-1.0, -1.0]), A0, B0, C0)
t, y, dt = 0., numpy.array([A0, B0, C0]), .02
while t <= 1.01:
print( (t, y) )
t, y = t + dt, y + dy( t, y, dt )
By the answers I saw, I realized that the solution is not really simple and will have to be solved by an Inverse Kinematics algorithm.
This site is an example and it is a just a start, although it still does not solve everything, since the point C is fixed and in the case of the truck it should move.
Based on this Analytic Two-Bone IK in 2D article, I made a fully functional model in Geogebra, where the nucleus consists of two simple mathematical equations.
I am interested in finding the t2' with the help of t1, t1' and t2.
Actually I am using some projection matrix T (this will be used to project point x and y which is clear in the Image attached) on x and y to get the points t1 and t2. Again I have to use the matrix T on x' and y' to find the t1' and t2'. But I don't want to exactly use the matrix T to do this just to avoid the lots of multiplication.
Is there any way to find t2' with the help of t1' and the result followed in the case of t1 and t1'.
T*x = t1
T*y = t2
t*x' = t1'
I am attaching picture for better visibility of the question.
If there is a linear relationship between the variables, you can use that same relationship after applying T because T, being a linear transformation, will preserve those relationships.
So, let's assume that the relationship you have verifies y - x = y' - x'. This assumption is consistent with the one you wrote in the comment because there y - x = (0, 1) and y'- x' = (0, 1) too.
So, applying T to both sides of the equation we have: T(y - x) = T(y' - x'). Now, using that T preserves linear combinations (i.e., T(u + sv) = T(u) + sT(v) for all vectors u,v and scalars s), we get: Ty - Tx = Ty' - Tx'. With your variable names we get: t2 - t1 = t2' - t1'. Solving for t2' leads to t2' = t2 - t1 + t1'.
If the actual relationship between x, y, x' and y' happens to be different but still linear you can apply the same reasoning to it.
Is there a way I can make a structure for 2 different functions using the same where clauses?
My code:
bonusColBullet :: Bonus -> Bullet -> World -> World
bonusColBullet bn#(Bonus{bnpos=pos}) b w#(World{bullets=bs, bonuses=bns, score=s})
| doBoxesCollide bnlp bnrp blp brp = w{bullets=delete b bs, bonuses=delete bn bns, score=incVal s}
| otherwise = w
where
blp = bpos' - bSizeH --bullet corners
brp = bpos' + bSizeH
bnlp = pos - bnSizeH --bonus obj corners
bnrp = pos + bnSizeH
bpos' = bpos b
incVal s#(Score{sval=sv, multiplier}) = s{sval=sv+multiplier}
enemyColBullet :: Enemy -> Bullet -> World -> World
enemyColBullet e#(Enemy{epos=pos}) b w#(World{bullets=bs, enemies=es, score=s})
| doBoxesCollide elp erp blp brp = w{bullets=delete b bs, enemies=delete e es, score=incVal s}
| otherwise = w
where
blp = bpos' - bSizeH -- bullet corners
brp = bpos' + bSizeH
elp = pos - eSizeH -- enemy corners
erp = pos + eSizeH
bpos' = bpos b
incVal s#(Score{sval=sv, multiplier}) = s{sval=sv+multiplier}
Because like this it looks very inefficient to me, so I figured there should be a way to only have to write the where clause ones and to somehow make it includeable for both functions?
If anyone could help me out on this it'd be much appreciated!
Best regards,
Skyfe.
Yep! You're right that this can be factored out. One cool feature in Haskell which may help you is ambiguity. Haskell has two sorts of ambiguity, and you can use either one in this case. The first sort of ambiguity is called parametric types, for example the list type [x] exists for all x no matter what x is, so there are actually a lot of different functions reverse :: [x] -> [x], one for every type which you can put inside those lists. The only problem is that sometimes you want a constraint on these parameters, so that you can do stuff with them (reverse is constrained by the total ambiguity; it can't do anything to the elements but can only reorder them and possibly drop or repeat them). The constrained ambiguity is a feature called type classes. Here's how I'd use them to fit your case:
type Size = ____ -- fill this in with whatever that type actually is.
class Sprite s where
kill :: s -> World -> World
position :: s -> Size
size :: s -> Size
instance Sprite Bullet where
kill b w = w{bullets = delete b (bullets w)}
position = bpos
size = const bSizeH -- constant for all bullets, right?
instance Sprite Bonus where
kill bn w = w{bonuses = delete bn (bonuses w)}
position = bnpos
size = const bnSizeH
instance Sprite Enemy where
kill e w = w{enemies = delete e (enemies w)}
position = epos
size = const eSizeH
Now you can write something more generic:
collides :: (Sprite x, Sprite y) => x -> y -> Bool
collides x y = doBoxesCollide (px - sx) (px + sx) (py - sy) (py + sy)
where px = position x
py = position y
sx = size x
sy = size y
addScore :: World -> World
addScore w = w{score = s{sval = sval s + multiplier s}} where s = score w
killCollision :: (Sprite x, Sprite y) => x -> y -> World -> World
killCollision x y = if collides x y then addScore . kill x . kill y else id
We've gone up from your 22 lines up to 27 lines, but we now have a lot of smaller parts interacting in clearer ways, and no repeating ourselves. It's an artistic choice whether this is worth it or not -- it often helps longer-term maintenance of a program, but if you're just trying to get a program out there often copy-paste is faster.
So now where you would have written bonusColBullet bn b w you can just write killCollision bn b w and it does the same thing, and so does killCollision e b w for enemies e. But you've got a little more power. Suppose you want to have enemies eat bonuses that they collide with: but you do not get a score for that. Then that would be if collides e bn then kill bn else id. Or you might decide that different sprites have different points which they are worth; then you add to the Sprite class points :: x -> Points (where Points is whatever Num type your points are -- presumably Integer but I didn't want to assume). You modify addScore to become:
addScore :: (Sprite x, Sprite y) => x -> y -> World -> World
addScore x y w = w{score = s{sval = newscore}
where s = score w
newscore = sval s + multiplier s * (points x + points y)
and you modify killCollision by replacing addScore with addScore x y. That's it. Now different enemies or bonuses can be worth different amounts. With a little more work, you can have an enemy who eats a bonus get the points from that bonus (so that if you kill them you still get the bonus points). Stuff like that.
Is there a way I can make a structure for 2 different functions using the same where clauses
where gives you a binding with local scope. So to share that between two functions, they have to be "inside" the where scope.
Easier is to float the where clause out. E.g. calculate it once and pass x and y to your functions.
main = do
let x = func a - c
y = func' b - c
someFunc x y
someOtherFunc x y
Some time ago I asked a question on math.stackexchange and got an answer. I have difficulties deriving an algorithm from that answer because my background is in design and hope some of you can help me.
The original question with visual sketch and possible answer are here:
https://math.stackexchange.com/questions/667432/triangle-with-two-constraints-each-corner-on-a-given-line
The question was: Given 3 3-dimensional lines (a, b and c) that coincide in a common point S and a given Point B on b, I'm looking for a point A on a and a point C on c where AB and BC have the same length and the angle ABC is 90 degrees.
I will have to implement this algorithm in an imperative language, any code in C++, Java, imperative pseudo-code or similar is fine.
Also, different approaches to this problem are equally welcome. Plus: Thanks for any hints, if the complete solution is indeed too time-consuming!
The two key formulas are
(I've replied the derivation for the formulas in the mathematics stack exchange site)
Substituting the first in the second gives in the end a 4th degree equation that is quite annoying to solve with a closed form. I've therefore used instead a trivial numerical solver in Python:
# function to solve (we look for t such that f(t)=0)
def f(t):
s = (t*cB - B2) / (t*ac - aB)
return s*s - 2*s*aB - t*t + 2*t*cB
# given f and an interval to search generates all solutions in the range
def solutions(f, x0, x1, n=100, eps=1E-10):
X = [x0 + i*(x1 - x0)/(n - 1) for i in xrange(n)]
Y = map(f, X)
for i in xrange(n-1):
if (Y[i]<0 and Y[i+1]>=0 or Y[i+1]<0 and Y[i]>=0):
xa, xb = X[i], X[i+1]
ya, yb = Y[i], Y[i+1]
if (xb - xa) < eps:
# Linear interpolation
# 0 = ya + (x - xa)*(yb - ya)/(xb - xa)
yield xa - ya * (xb - xa) / (yb - ya)
else:
for x in solutions(f, xa, xb, n, eps):
yield x
The search algorithm samples the function in the interval and when it finds two adjacent samples that are crossing the f=0 line repeats the search recursively between those two samples (unless the interval size is below a specified limit, approximating the function with a line and computing the crossing point in that case).
I've tested the algorithm generating random problems and solving them with
from random import random as rnd
for test in xrange(1000):
a = normalize((rnd()-0.5, rnd()-0.5, rnd()-0.5))
b = normalize((rnd()-0.5, rnd()-0.5, rnd()-0.5))
c = normalize((rnd()-0.5, rnd()-0.5, rnd()-0.5))
L = rnd() * 100
B = tuple(x*L for x in b)
aB = dot(a, B)
cB = dot(c, B)
B2 = dot(B, B)
ac = dot(a, c)
sols = list(solutions(f, -1000., 1000.))
And there are cases in which the solutions are 0, 1, 2, 3 or 4. For example the problem
a = (-0.5900900304960981, 0.4717596600172049, 0.6551614908475357)
b = (-0.9831451620384042, -0.10306322574446096, 0.15100848274062748)
c = (-0.6250439408232388, 0.49902426033920616, -0.6002456660677057)
B = (-33.62793897729328, -3.5252208930692497, 5.165162011403056)
has four distinct solutions:
s = 57.3895941365 , t = -16.6969433689
A = (-33.865027354189415, 27.07409541837935, 37.59945205363035)
C = (10.436323283003153, -8.332179814593692, 10.022267893763457)
|A - B| = 44.5910029061
|C - B| = 44.5910029061
(A - B)·(C - B) = 1.70530256582e-13
s = 43.619078237 , t = 32.9673082734
A = (-25.739183207076163, 20.5777215193455, 28.577540327140607)
C = (-20.606016281518986, 16.45148662649085, -19.78848391300571)
|A - B| = 34.5155582156
|C - B| = 34.5155582156
(A - B)·(C - B) = 1.13686837722e-13
s = -47.5886624358 , t = 83.8222109697
A = (28.08159526800866, -22.450411211385674, -31.17825902887765)
C = (-52.39256507303229, 41.82931682916268, -50.313918854788845)
|A - B| = 74.0747844969
|C - B| = 74.0747844969
(A - B)·(C - B) = 4.54747350886e-13
s = 142.883074325 , t = 136.634726869
A = (-84.31387768560096, 67.4064705656035, 93.61148799140805)
C = (-85.40270813540043, 68.1840435123674, -82.01440263735996)
|A - B| = 124.189861967
|C - B| = 124.189861967
(A - B)·(C - B) = -9.09494701773e-13
Write two quadratic equations for lambda, mu unknowns (just above matrix forms).
Solve this system with paper, pen and head, or with any mathematical software like Maple, Mathematica, Matlab, Derive etc. You will have 4th order equation. It has closed-form solution - apply Ferrari or Kardano method and get real roots, find mu, lambda, then point coordinates.
I have a vector X of 20 real numbers and a vector Y of 20 real numbers.
I want to model them as
y = ax^2+bx + c
How to find the value of 'a' , 'b' and 'c'
and best fit quadratic equation.
Given Values
X = (x1,x2,...,x20)
Y = (y1,y2,...,y20)
i need a formula or procedure to find following values
a = ???
b = ???
c = ???
Thanks in advance.
Everything #Bartoss said is right, +1. I figured I just add a practical implementation here, without QR decomposition. You want to evaluate the values of a,b,c such that the distance between measured and fitted data is minimal. You can pick as measure
sum(ax^2+bx + c -y)^2)
where the sum is over the elements of vectors x,y.
Then, a minimum implies that the derivative of the quantity with respect to each of a,b,c is zero:
d (sum(ax^2+bx + c -y)^2) /da =0
d (sum(ax^2+bx + c -y)^2) /db =0
d (sum(ax^2+bx + c -y)^2) /dc =0
these equations are
2(sum(ax^2+bx + c -y)*x^2)=0
2(sum(ax^2+bx + c -y)*x) =0
2(sum(ax^2+bx + c -y)) =0
Dividing by 2, the above can be rewritten as
a*sum(x^4) +b*sum(x^3) + c*sum(x^2) =sum(y*x^2)
a*sum(x^3) +b*sum(x^2) + c*sum(x) =sum(y*x)
a*sum(x^2) +b*sum(x) + c*N =sum(y)
where N=20 in your case. A simple code in python showing how to do so follows.
from numpy import random, array
from scipy.linalg import solve
import matplotlib.pylab as plt
a, b, c = 6., 3., 4.
N = 20
x = random.rand((N))
y = a * x ** 2 + b * x + c
y += random.rand((20)) #add a bit of noise to make things more realistic
x4 = (x ** 4).sum()
x3 = (x ** 3).sum()
x2 = (x ** 2).sum()
M = array([[x4, x3, x2], [x3, x2, x.sum()], [x2, x.sum(), N]])
K = array([(y * x ** 2).sum(), (y * x).sum(), y.sum()])
A, B, C = solve(M, K)
print 'exact values ', a, b, c
print 'calculated values', A, B, C
fig, ax = plt.subplots()
ax.plot(x, y, 'b.', label='data')
ax.plot(x, A * x ** 2 + B * x + C, 'r.', label='estimate')
ax.legend()
plt.show()
A much faster way to implement solution is to use a nonlinear least squares algorithm. This will be faster to write, but not faster to run. Using the one provided by scipy,
from scipy.optimize import leastsq
def f(arg):
a,b,c=arg
return a*x**2+b*x+c-y
(A,B,C),_=leastsq(f,[1,1,1])#you must provide a first guess to start with in this case.
That is a linear least squares problem. I think the easiest method which gives accurate results is QR decomposition using Householder reflections. It is not something to be explained in a stackoverflow answer, but I hope you will find all that is needed with this links.
If you never heard about these before and don't know how it connects with you problem:
A = [[x1^2, x1, 1]; [x2^2, x2, 1]; ...]
Y = [y1; y2; ...]
Now you want to find v = [a; b; c] such that A*v is as close as possible to Y, which is exactly what least squares problem is all about.