?? jlsform01.cpp
字號:
// JlsForm01.cpp: implementation of the JlsForm01 class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "JlsForm01.h"
void JlsForm01A( const double * x, const JVector * rl, const JVector * rw, JMatrix * A )
{
//
// A = r + m * rl - rw
//
// Parameters:
// x (in) x0, x1, x2, l0, l1, l2, l3 dim[7]
// rl (in) the point local to <x>
// rw (in) the point as global as <x>
// A (out) the differential vector dim[3][1]
//
JRotation m( x + 3 ) ;
JVector v( m * *rl ) ;
(*A)[0][0] = x[0] + v.x - rw->x ;
(*A)[1][0] = x[1] + v.y - rw->y ;
(*A)[2][0] = x[2] + v.z - rw->z ;
}
void JlsForm01Ax( const double * x, const JVector * rl, const JVector * rw, JMatrix * Ax )
{
//
// A = r + m * rl - rw
//
// Parameters:
// x (in) x0, x1, x2, l0, l1, l2, l3 dim[7]
// rl (in) the point local to <x>
// rw (in) the point as global as <x>
// Ax (out) the differential maxtrix dim[3][7]
//
(*Ax)[0][0] = 1 ;
(*Ax)[0][1] = 0 ;
(*Ax)[0][2] = 0 ;
(*Ax)[0][3] = 4 * x[3] * rl->x - 2 * x[6] * rl->y + 2 * x[5] * rl->z ;
(*Ax)[0][4] = 4 * x[4] * rl->x + 2 * x[5] * rl->y + 2 * x[6] * rl->z ;
(*Ax)[0][5] = 2 * x[4] * rl->y + 2 * x[3] * rl->z ;
(*Ax)[0][6] = - 2 * x[3] * rl->y + 2 * x[4] * rl->z ;
(*Ax)[1][0] = 0 ;
(*Ax)[1][1] = 1 ;
(*Ax)[1][2] = 0 ;
(*Ax)[1][3] = 2 * x[6] * rl->x + 4 * x[3] * rl->y - 2 * x[4] * rl->z ;
(*Ax)[1][4] = 2 * x[5] * rl->x - 2 * x[3] * rl->z ;
(*Ax)[1][5] = 2 * x[4] * rl->x + 4 * x[5] * rl->y + 2 * x[6] * rl->z ;
(*Ax)[1][6] = 2 * x[3] * rl->x + 2 * x[5] * rl->z ;
(*Ax)[2][0] = 0 ;
(*Ax)[2][1] = 0 ;
(*Ax)[2][2] = 1 ;
(*Ax)[2][3] = -2 * x[5] * rl->x + 2 * x[4] * rl->y + 4 * x[3] * rl->z ;
(*Ax)[2][4] = 2 * x[6] * rl->x + 2 * x[3] * rl->y ;
(*Ax)[2][5] = -2 * x[3] * rl->x + 2 * x[6] * rl->y ;
(*Ax)[2][6] = 2 * x[4] * rl->x + 2 * x[5] * rl->y + 4 * x[6] * rl->z ;
}
void JlsForm01Axjx( const double * x, const JVector * rl, const JVector * rw, JMatrix ** Axjx )
{
//
// A = r + m * rl - rw
//
// Parameters:
// x (in) x0, x1, x2, l0, l1, l2, l3 dim[7]
// rl (in) the point local to <x>
// rw (in) the point as global as <x>
// Axjx (out) the 7 differential maxtrix dim[3][7] * 7
//
for ( int i=0 ; i<7 ; i++ ) Axjx[i]->Zero() ;
(*Axjx[3])[0][3] = 4 * rl->x ; (*Axjx[3])[0][5] = 2 * rl->z ; (*Axjx[3])[0][6] =-2 * rl->y ;
(*Axjx[3])[1][3] = 4 * rl->y ; (*Axjx[3])[1][4] =-2 * rl->z ; (*Axjx[3])[1][6] = 2 * rl->x ;
(*Axjx[3])[2][3] = 4 * rl->z ; (*Axjx[3])[2][4] = 2 * rl->y ; (*Axjx[3])[2][5] =-2 * rl->x ;
(*Axjx[4])[0][4] = 4 * rl->x ; (*Axjx[4])[0][5] = 2 * rl->y ; (*Axjx[4])[0][6] = 2 * rl->z ;
(*Axjx[4])[1][3] =-2 * rl->z ; (*Axjx[4])[1][5] = 2 * rl->x ;
(*Axjx[4])[2][3] = 2 * rl->y ; (*Axjx[4])[2][6] = 2 * rl->x ;
(*Axjx[5])[0][3] = 2 * rl->z ; (*Axjx[5])[0][4] = 2 * rl->y ;
(*Axjx[5])[1][4] = 2 * rl->x ; (*Axjx[5])[1][5] = 4 * rl->y ; (*Axjx[5])[1][6] = 2 * rl->z ;
(*Axjx[5])[2][3] =-2 * rl->x ; (*Axjx[5])[2][6] = 2 * rl->y ;
(*Axjx[6])[0][3] =-2 * rl->y ; (*Axjx[6])[0][4] = 2 * rl->z ;
(*Axjx[6])[1][3] = 2 * rl->x ; (*Axjx[6])[1][5] = 2 * rl->z ;
(*Axjx[6])[2][4] = 2 * rl->x ; (*Axjx[6])[2][5] = 2 * rl->y ; (*Axjx[6])[2][6] = 4 * rl->z ;
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -