function [t_s,t_c]=RemoteCenterOfStiffness(bKt)
% RemoteCenterOfStiffness.m
%  usage: t=RemoteCenterOfStiffness(bKt)
% Works off of bKt
% Based on "Normal Forms of Stiffness and Compliance Matrices" by Josip
% Loncaric, IEEE Journal of Robotics and Automation, Vol RA-3, No 6,
% December 1987, 567-572.

C=bKt(1:3,1:3);  % Loncaric flips the state vector, so C is the translational stiffness matrix
B=bKt(1:3,4:6);  % Translational/Rotational Coupling matrix (Actually B' in Loncaric's work)
A=bKt(4:6,4:6);  % Rotational stiffness matrix

Bas=1/2*(B-B');  % Anti-Symmetric portion of the translational/rotational coupling matrix
b=[Bas(3,2);Bas(1,3);Bas(2,1)]; % -bx of Loncaric's Eqn 18.
t_s=-2*inv(trace(C)*eye(3)-C)*b;   % Vector from current origin of the stiffness matrix to the center of stiffness.  Negative comes from difference in mine and Loncaric's vector formation

%Stiffness matrix at center of stiffness cKt=bcTv' * bKt * bcTv
tX_s = [0 -t_s(3) t_s(2);t_s(3) 0 -t_s(1); -t_s(2) t_s(1) 0];
bcTv = [eye(3) tX_s;zeros(3) eye(3)];
cKt = bcTv' * bKt * bcTv

%Rotation matrix to diagonalize the off-diagonal blocks (just for checking)
% By=cKt(1:3,4:6);
% [R,D]=eig(By);
% b2cTv=[R tX*R;zeros(3) R];
% c2Kt = b2cTv' * bKt * b2cTv

%Compliance Center
bCt=inv(bKt);
F=bCt(1:3,1:3); 
D=bCt(4:6,4:6);
E=bCt(1:3,4:6);

Eas=1/2*(E-E');
e=[Eas(3,2);Eas(1,3);Eas(2,1)];
t_c=2*inv(trace(D)*eye(3)-D)*e;

%Compliance matrix at center of compliance cCt = cbTv * bCt * cbTv'
tX_c = -[0 -t_c(3) t_c(2);t_c(3) 0 -t_c(1); -t_c(2) t_c(1) 0];
cbTv = [eye(3) tX_c;zeros(3) eye(3)];
cCt = cbTv * bCt * cbTv'