�{�߂ł́A�����̕����G���W�����̗p���Ă���S���͌v�Z�ɂ��ƂÂ��V�~�����[�V������@���ȒP�ɐ�������B
�Ȃ��C�{�߂���шȍ~�̐߂ɂ����āC�Y����$i$�C$j$�����̂�S�����Q�Ƃ���ꍇ�ƃx�N�g���E�s��̐������w������ꍇ�̗����ɗp����̂Œ��ӂ��ꂽ���D

���̂̐���$N^\mathrm{B}$�Ƃ��C����$i$\,($i = 1,2,\dots,N^\mathrm{B}$)�̑��x���^�����W�n�ŕ\�������x�N�g����$\bm{v}_i$�Ƃ���D
������$\bm{v}_i$�͕��i���x��x�Cy�Cz�����C�p���x��x�Cy�Cz�����̏��ɕ���6�����x�N�g���ł���D
�����$\bm{v}_i$��S���̂ɂ��ĘA�������x�N�g����$\bm{v} \in \mathbb{R}^m$ ($m = 6 N^\mathrm{B}$)�Ə����D
�I�C���[����K�p����ƁC�n�̎������̑��x$v'$��
\begin{align}
\bm{v}' &= \bm{v} + h (M^{-1} (\bm{f} + \bm{f}^\mathrm{ext}) + \bm{c})
\label{eq_body_velupdate}
\end{align}
�Ə�����D
������$\bm{f}$�͍��̂ɍ�p����S���́C$\bm{f}^\mathrm{ext}$�͍S���͈ȊO�̒l�����m�ł���́i�d�͂����̒��Ɋ܂܂��j�C
$\bm{c}$�̓R���I���͂Ɖ��S�͂ɗR����������x�ł���D
��������e���̂Ɋւ�����i�����Ɖ�]�������琬��$6 N^\mathrm{B}$�����x�N�g���ŁC���x�Ɠ��l�ɍ��̂̉^�����W�n�\���ł���D
�܂��C$M$�͊e���̂̎��ʂƊ������[�����g���琬��u���b�N�Ίp�s��C$h$�̓X�e�b�v���ł���D
%���āC$i_1 \xrightarrow{j} i_2$�Ȃ鍄�̂ƍS���̑g���l����D
%�\�P�b�g�ɑ΂���v���O�̑��x���\�P�b�g�̍��W�n�ŕ\�킵�����̂�$w_{j}$�Ƃ�����
\begin{figure}[t]
\begin{center}
\includegraphics[width=70mm]{fig/joint.eps}
\end{center}
\caption{���̂ƍS���̊֌W}
\label{fig_joint}
\end{figure}
����C�S���̐���$N^\mathrm{J}$�Ƃ���D
�S���͓�̍��̂̑g�ɍS���͂���p���C���̊Ԃ̑��Ή^���ɐ�����������v�f�ł���D
�ł��T�^�I�Ȃ��͎̂�X�̊֐߁i��]�֐߁C�����֐߁C���ʊ֐߂Ȃǁj�ł��邪�C�ڐG���q����o�l�E�_���p�Ȃǂ��S���Ƃ��Ĉ�����D
����$i_1$�C$i_2$�ԂɍS��$j$\,($j = 1,2,\dots,N^\mathrm{J}$)����p����Ƃ��̈ʒu�֌W��\figlabel\ref{fig_joint}�Ɏ����D
���̂Ƃ��C����$i_1$�ɂ̓\�P�b�g�C����$i_2$�ɂ̓v���O�ƌĂ΂����W�n�����t������i�\�P�b�g/�v���O�Ƃ����ď̂�Springhead�ɏ�����j�D
�����Ń\�P�b�g���猩���v���O�̑��Α��x���\�P�b�g���W�n�ŕ\�������x�N�g����$\bm{w}_j$�i$\bm{v}_i$���l���i���x�Ɗp���x��6�����x�N�g���j�Ƃ����ƁC
\begin{align}
\bm{w}_{j} = J_{i_2,j} \bm{v}_{i_2} - J_{i_1,j} \bm{v}_{i_1}
\end{align}
�Ə�����D������$J_{i_1,j}$�C$J_{i_2,j}$�̓\�P�b�g/�v���O�̈ʒu�⍄�̓��m�̈ʒu�֌W���猈�܂郄�R�r�s��ł���D
�S�S���ɂ���$\bm{w}_j$��A�������x�N�g����$\bm{w} \in \mathbb{R}^n$ ($n = 6 N^\mathrm{J}$)�Ƃ����ƁC��̎��͂܂Ƃ߂�
\begin{align}
\bm{w} = J \, \bm{v}
\end{align}
�Ə�����D�{�e�ł�$\bm{w}_j$��$\bm{w}$���S�����x�ƌĂԁD

�S�������̂ɍ�p����S���͂�$\bm{\lambda} \in \mathbb{R}^n$�Ə����D
���z�d���̌�������$\bm{\lambda}^\mathsf{T} \bm{w} = \bm{\lambda}^\mathsf{T} J \bm{v} = \bm{f}^\mathsf{T} \bm{v}$�������邩��C
$\bm{f} = J^\mathsf{T} \bm{\lambda}$�𓾂�D
�������(\ref{eq_body_velupdate})�ɑ������ƁC�������̍S�����x$\bm{w}' = J \bm{v}'$�͍S����$\bm{\lambda}$�Ɋւ���ꎟ���Ƃ��Ď��̂悤�ɏ�����D
\begin{align}
&\bm{w}' = A \bm{\lambda} + \bm{b},
\label{eq_lcp_linear} \\
&\;\; A = h (J M^{-1} J^\mathsf{T}), \, \bm{b} = J (\bm{v} + h (M^{-1}\bm{f}^\mathrm{ext} + \bm{c}) ). \notag
\end{align}
�S�����x$\bm{w}'$�ƍS����$\bm{\lambda}$�̊e�����ɂ́C�S���̎�ނɉ����ĈقȂ�㐔�������ۂ�����D
������ȉ��̕\�ɂ܂Ƃ߂�D

\begin{flushleft}
\begin{tabular}{lll}
\hline
���			& �\�L								& ����														\\ \hline
���S��			& $i \in \mathcal{I}^\mathrm{f}$	& $\lambda_i = 0$											\\
��{�S��		& $i \in \mathcal{I}^\mathrm{e}$	& $w'_i = 0$												\\
���␫�S��		& $i \in \mathcal{I}^\mathrm{c}$	& $w'_i \ge 0, \, \lambda_i \ge 0, \, w'_i \lambda_i = 0$	\\
���`�S��		& $i \in \mathcal{I}^\mathrm{s}$	& $w'_i = \alpha_i \lambda_i + \beta_i$						\\ \hline
\end{tabular}
\end{flushleft}
\vspace{-2.7zh}
\begin{align}
\label{eq_lcp_cond}
\end{align}
\noindent 
�����ŔC�ӂ�$i \in \{1,2,\dots,n\}$��$\mathcal{I}^\mathrm{f}$, $\mathcal{I}^\mathrm{e}$, $\mathcal{I}^\mathrm{c}$�����$\mathcal{I}^\mathrm{s}$��
�����ꂩ�Ɋ܂܂�C���d���͂Ȃ����̂Ƃ���D
���S���͕����ʂ�S�����ۂ���Ȃ����Əꍇ�������C�S���͂𐶂��Ȃ��D
���R��]����֐߂̎������̊p���x�����Ȃǂ�����ɊY������D
��{�S���͂���Ƌt�̏ꍇ�ŁC�S�����x��$0$�ƂȂ�悤�ɍS���͂�������D
���␫�S���͐ڐG����͈͍S���������ꍇ�Ɍ����D
�Ō�ɐ��`�S���͊֐߂Ƀo�l�E�_���p�i���邢��PD�����j��������ꍇ�Ɍ����D
���`�S���͕ό`�ɂ���{�S���ɋA���ł���D����ɂ��Ă�\ref{subsec_spring_damper}�߂ŏڂ����q�ׂ�D

��Ƃ���1���R�x�̉�]�֐߂������������ꍇ�́C$\bm{w}_j$��6�����̂������ix�Cy�Cz�Ɖ�]x�Cy�����Ɋ�{�S���C
��]$z$�����ɂ͖��S���i���R��]�j���邢�͐��`�S���i�A�N�`���G�[�^����j���ۂ��΂悢�D
���̂Ƃ�$z$������]���ƂȂ�D
�܂��C�ڐG����������ɂ́C�܂��ڐG�_�ɂ����Ċe���̂Ƀ\�P�b�g�ƃv���O��x�����ڐG�@���������悤�Ɏ��t���C
���ix�����ɑ��␫�S���Cy�Cz�����Ɋ�{�S���C��]x�Cy�Cz�����𖳍S���Ƃ���D
���������̏ꍇ�͐Î~���C�W����$\infty$�ƂȂ�i���iy�Cz�����𖳍S���Ƃ���Ζ��C�W��$0$�ƂȂ�j�D
����ʂ̐Î~���C�W���⓮���C��\������ɂ͕��iy�Cz�����Ɋւ��đ��␫�S���Ɏ����������ۂ����ƂɂȂ�D

�ȏ���C�S���͂����߂���͎�(\ref{eq_lcp_linear})(\ref{eq_lcp_cond})�ŗ^��������`���␫���(linear complimentarity problem, LCP)�ƂȂ�D
�����V�~�����[�V�����ɂ�����LCP�̉�@�Ƃ��Ă͓���Lemke�@���p�����Ă�����\cite{Baraff94}�C
��ɂ��v�Z�R�X�g�̒Ⴂ������@���L���p������悤�ɂȂ���\cite{Potra02}�D
\begin{algorithm}[t]
%\caption{Gauss-Seidel method for constraint force calculation}
\caption{�K�E�X�E�U�C�f���@�ɂ��S���͌v�Z}
\label{table_algo_lcp}
%\normalsize
\begin{algorithmic}[1]
%\State $\bm{\lambda} \gets \bm{\lambda}^\mathrm{ini}, \, \bm{w} \gets A \bm{\lambda}^\mathrm{ini} + \bm{b}$
\State $\bm{\lambda} \gets 0, \, \bm{w}' \gets \bm{b}$
\For{$k = 1$ to $K$}
	\State \Comment{ 1. constraint force update}
	\For{$i = 1$ to $n$}
		\If{$i \in \mathcal{I}^\mathrm{f}$}
			\State continue
		\EndIf
		\State $\lambda'_i \gets \lambda_i - A_{ii}^{-1}\,w'_i$
		\If{$i \in \mathcal{I}^\mathrm{c}$ and $\lambda'_i < 0$}
			\State $\lambda'_i \gets 0$
		\EndIf
		\State $\delta \lambda_i \gets \lambda'_i - \lambda_i$
		\State \Comment{ 2. velocity update}
		\For{$j=1$ to $m$}
			\State $\delta v_j \gets (M^{-1} J^\mathsf{T})_{ji} \delta \lambda_i, \, v_j \gets v_j + \delta v_j$
			\State \Comment{ 3. constraint error update}
			\For{$i=1$ to $n$}
				\State $w'_i \gets w'_i + J_{ij} \delta v_j$
			\EndFor
		\EndFor
	\EndFor
\EndFor
\State \Return $\lambda$
\end{algorithmic}
\end{algorithm}
Springhead���̗p���Ă���K�E�X-�U�C�f���@�Ɋ�Â��A���S���Y����\textbf{Algorithm}\,\ref{table_algo_lcp}�Ɏ����D
�O�q�̂悤�ɐ��`�S���͊�{�S���ɋA���ł���̂Ŗ{�A���S���Y���ł͒��ڈ����Ă��Ȃ��D
�e�����ɂ����āC�S���̊e���������񂵁C$i \in \mathcal{I}^\mathrm{e} \cup \mathcal{I}^\mathrm{c}$�ł����
$w'_i$��$0$�ƂȂ�悤��$\lambda_i$���C������Ƃ����葱�����s���D
�����āC���␫�S���i$i \in \mathcal{I}^\mathrm{c}$�j�ł���΍X�V���$\lambda_i \ge 0$�ƂȂ�悤�Ɏˉe������{���D
�K�E�X�E�U�C�f���@�̔�����$K$�̓��[�U���ݒ肷��D
���R$K$��傫���ݒ肷��قǌv�Z���x�͌��シ�邪�C�v�Z���Ԃ͔��I�ɑ��傷��D
%�Ȃ��C�����ł͌n�S�̂�\����K�͂ȍs��E�x�N�g���Ɋւ��鉉�Z�Ƃ��Ď葱�����L�q���Ă��邪�C
%�����ɂ����Ă�����

