77 lines
2.2 KiB
Mathematica
77 lines
2.2 KiB
Mathematica
|
function J = CalcRigidJacobian(M, V, p_local, p_global)
|
||
|
|
|
||
|
|
n = size(M, 1)/3;
|
||
|
|
|
||
|
|
% Get the current 3D shape (not affected by global transform, as this
|
||
|
|
% is how the Jacobian was derived (for derivation please see
|
||
|
|
% ../derivations/orthoJacobian
|
||
|
|
shape3D = GetShape3D(M, V, p_local);
|
||
|
|
|
||
|
|
% Get the rotation matrix corresponding to current global orientation
|
||
|
|
R = Euler2Rot(p_global(2:4));
|
||
|
|
s = p_global(1);
|
||
|
|
|
||
|
|
% Rigid Jacobian is laid out as follows
|
||
|
|
% dx_1/ds, dx_1/dr1, dx_1/dr2, dx_1/dr3, dx_1/dtx, dx_1/dty
|
||
|
|
% dx_2/ds, dx_2/dr1, dx_2/dr2, dx_2/dr3, dx_2/dtx, dx_2/dty
|
||
|
|
% ...
|
||
|
|
% dx_n/ds, dx_n/dr1, dx_n/dr2, dx_n/dr3, dx_n/dtx, dx_n/dty
|
||
|
|
% dy_1/ds, dy_1/dr1, dy_1/dr2, dy_1/dr3, dy_1/dtx, dy_1/dty
|
||
|
|
% ...
|
||
|
|
% dy_n/ds, dy_n/dr1, dy_n/dr2, dy_n/dr3, dy_n/dtx, dy_n/dty
|
||
|
|
|
||
|
|
J = zeros(n*2, 6);
|
||
|
|
|
||
|
|
% dx/ds = X * r11 + Y * r12 + Z * r13
|
||
|
|
% dx/dr1 = s*(r13 * Y - r12 * Z)
|
||
|
|
% dx/dr2 = -s*(r13 * X - r11 * Z)
|
||
|
|
% dx/dr3 = s*(r12 * X - r11 * Y)
|
||
|
|
% dx/dtx = 1
|
||
|
|
% dx/dty = 0
|
||
|
|
|
||
|
|
% dy/ds = X * r21 + Y * r22 + Z * r23
|
||
|
|
% dy/dr1 = s * (r23 * Y - r22 * Z)
|
||
|
|
% dy/dr2 = -s * (r23 * X - r21 * Z)
|
||
|
|
% dy/dr3 = s * (r22 * X - r21 * Y)
|
||
|
|
% dy/dtx = 0
|
||
|
|
% dy/dty = 1
|
||
|
|
|
||
|
|
% set the Jacobian for x's
|
||
|
|
|
||
|
|
% with respect to scaling factor
|
||
|
|
J(1:n,1) = shape3D * R(1,:)';
|
||
|
|
|
||
|
|
% with respect to angular rotation around x, y, and z axes
|
||
|
|
|
||
|
|
% Change of x with respect to change in axis angle rotation
|
||
|
|
dxdR = [ 0, R(1,3), -R(1,2);
|
||
|
|
-R(1,3), 0, R(1,1);
|
||
|
|
R(1,2), -R(1,1), 0];
|
||
|
|
|
||
|
|
J(1:n,2:4) = s*(dxdR * shape3D')';
|
||
|
|
|
||
|
|
% with respect to translation
|
||
|
|
J(1:n,5) = 1;
|
||
|
|
J(1:n,6) = 0;
|
||
|
|
|
||
|
|
% set the Jacobian for y's
|
||
|
|
|
||
|
|
% with respect to scaling factor
|
||
|
|
J(n+1:end,1) = shape3D * R(2,:)';
|
||
|
|
|
||
|
|
% with respect to angular rotation around x, y, and z axes
|
||
|
|
|
||
|
|
% Change of y with respect to change in axis angle rotation
|
||
|
|
dydR = [ 0, R(2,3), -R(2,2);
|
||
|
|
-R(2,3), 0, R(2,1);
|
||
|
|
R(2,2), -R(2,1), 0];
|
||
|
|
|
||
|
|
J(n+1:end,2:4) = s*(dydR * shape3D')';
|
||
|
|
|
||
|
|
% with respect to translation
|
||
|
|
J(n+1:end,5) = 0;
|
||
|
|
J(n+1:end,6) = 1;
|
||
|
|
|
||
|
|
end
|
||
|
|
|