/*! \fn void MultiNSH(int n, Real *tstop, Real *mratio, Real etavk,
* Real *uxNSH, Real *uyNSH, Real *wxNSH, Real *wyNSH)
* \brief Multi-species NSH equilibrium
*
* Input: # of particle types (n), dust stopping time and mass ratio array, and
* drift speed etavk.
* Output: gas NSH equlibrium velocity (u), and dust NSH equilibrium velocity
* array (w).
*/
void MultiNSH(int n, Real *tstop, Real *mratio, Real etavk,
Real *uxNSH, Real *uyNSH, Real *wxNSH, Real *wyNSH)
{
int i,j;
Real *Lambda1,**Lam1GamP1, **A, **B, **Tmp;
Lambda1 = (Real*)calloc_1d_array(n, sizeof(Real)); /* Lambda^{-1} */
Lam1GamP1=(Real**)calloc_2d_array(n, n, sizeof(Real)); /* Lambda1*(1+Gamma) */
A = (Real**)calloc_2d_array(n, n, sizeof(Real));
B = (Real**)calloc_2d_array(n, n, sizeof(Real));
Tmp = (Real**)calloc_2d_array(n, n, sizeof(Real));
/* definitions */
for (i=0; i<n; i++){
for (j=0; j<n; j++)
Lam1GamP1[i][j] = mratio[j];
Lam1GamP1[i][i] += 1.0;
Lambda1[i] = 1.0/(tstop[i]+1.0e-16);
for (j=0; j<n; j++)
Lam1GamP1[i][j] *= Lambda1[i];
}
/* Calculate A and B */
MatrixMult(Lam1GamP1, Lam1GamP1, n,n,n, Tmp);
for (i=0; i<n; i++) Tmp[i][i] += 1.0;
InverseMatrix(Tmp, n, B);
for (i=0; i<n; i++)
for (j=0; j<n; j++)
B[i][j] *= Lambda1[j];
MatrixMult(Lam1GamP1, B, n,n,n, A);
/* Obtain NSH velocities */
*uxNSH = 0.0; *uyNSH = 0.0;
for (i=0; i<n; i++){
wxNSH[i] = 0.0;
wyNSH[i] = 0.0;
for (j=0; j<n; j++){
wxNSH[i] -= B[i][j];
wyNSH[i] -= A[i][j];
}
wxNSH[i] *= 2.0*etavk;
wyNSH[i] *= etavk;
*uxNSH -= mratio[i]*wxNSH[i];
*uyNSH -= mratio[i]*wyNSH[i];
wyNSH[i] += etavk;
}
free(Lambda1);
free_2d_array(A); free_2d_array(B);
free_2d_array(Lam1GamP1); free_2d_array(Tmp);
return;
}
bool RGBColorSystem::Data::ValidateParameters( const FVector& x, const FVector& y, const FVector& Y )
{
try
{
volatile Vector M_ = InverseMatrix( SetupMatrix( x, y, Y ) );
return true;
}
catch ( ... )
{
return false;
}
}
Matrix Inverse(Matrix &M)
{
int dim = M.size();
Matrix CofactorMatrix(dim);
Matrix InverseMatrix(dim);
for(int l=0;l<dim;l++) CofactorMatrix[l].resize(dim);
for(int l=0;l<dim;l++) InverseMatrix[l].resize(dim);
for(int i = 0; i < dim; i++)
{
for(int m = 0; m < dim; m++)
{
Matrix Cofactor(dim-1);
for(int l=0;l<dim-1;l++) Cofactor[l].resize(dim-1);
int col=0, row=0;
for(int j = 0; j < dim; j++)
{
if(j != i)
{
row = 0;
for(int k = 0; k < dim; k++)
{
if (k != m)
{
Cofactor[row][col] = M[k][j];
row++;
}
}
col++;
}
}
CofactorMatrix[i][m] = ((i+m)%2==1?-1.0:1.0) * Det(Cofactor);
}
}
for(int i=0; i< dim; i++)
{
InverseMatrix[i] = (1.0/Det(M))*CofactorMatrix[i];
}
return InverseMatrix;
}
int main(int argc, char* argv[])
{
float *a[3],*b[3];
int i,j;
for(i=0;i<3;i++)
a[i] = (float *)malloc(3);
for(i=0;i<3;i++)
b[i] = (float *)malloc(3);
printf("hi");
a[0][0]=1;
a[0][1]=2;
a[0][2]=3;
a[1][0]=0;
a[1][1]=1;
a[1][2]=4;
a[2][0]=5;
a[2][1]=6;
a[2][2]=0;
if(InverseMatrix(a,b) == -1)
return 0;
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
{
printf("%f ",c[i][j]);
}
printf("\n");
}
printf("\n\n\n");
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
{
printf("%f ",b[i][j]);
}
printf("\n");
}
for(i=0;i<3;i++)
{
free(a[i]);
free(b[i]);
}
return 0;
}
void kalmanProcess(kalman_s *mykalman)
{
int i,j;
for(i=0;i<NM;i++)
memset(mykalman->temp_4_4[i],0,sizeof(double)*NM);
//第一个公式：x(k|k-1) = Ax(k-1|k-1)
multiplyMatrix(mykalman->A,NS,NS,mykalman->X,NS,1,mykalman->temp_1); //X=A*X
for (i=0;i<NS;i++)
mykalman->X[i][0]=mykalman->temp_1[i][0];
//第二个公式： P = A*P*A'+Q
multiplyMatrix(mykalman->A,NS,NS,mykalman->P,NS,NS,mykalman->temp_2_1); //temp_2_1 = A*P
transpositionMatrix(mykalman->A, mykalman->temp_2, NS, NS); //temp_2 = A'
multiplyMatrix(mykalman->temp_2_1,NS,NS,mykalman->temp_2,NS,NS,mykalman->P); //P = A*P*A’
addMatrix(mykalman->P,NS,NS,mykalman->Q,NS,NS,mykalman->P); //P = A*P*A’+Q
//第三个公式： X = X+K*[Z-H*X]
multiplyMatrix(mykalman->H,NM,NS,mykalman->X,NS,1,mykalman->temp_3_1); //temp_3_1=H*X
subMatrix(mykalman->Z,NM,1,mykalman->temp_3_1,NM,1,mykalman->temp_3_1); //temp_3_1=Z-H*X
multiplyMatrix(mykalman->K,NS,NM,mykalman->temp_3_1,NM,1,mykalman->temp_3_2); //temp_3_2 = K*(Z-H*X)
addMatrix(mykalman->X,NS,1,mykalman->temp_3_2,NS,1,mykalman->X); //X = X+ K*(Z-H*X)
//第四个公式：K = P*H'/[H*P*H'+R]
transpositionMatrix(mykalman->H,mykalman->temp_4_3,NM,NS); //temp_4_3 = H'
multiplyMatrix(mykalman->P,NS,NS,mykalman->temp_4_3,NS,NM,mykalman->temp_4_1); //temp_4_1 = P*H'
multiplyMatrix(mykalman->H,NM,NS,mykalman->temp_4_1,NS,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H'
addMatrix(mykalman->temp_4_2,NM,NM,mykalman->R,NM,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H'+R
InverseMatrix(mykalman->temp_4_2, mykalman->temp_4_4, NM,NM); //temp_4_4=~(H*P*H'+R)
multiplyMatrix(mykalman->temp_4_1,NS,NM,mykalman->temp_4_4,NM,NM,mykalman->K); //K = P*H'*~(H*P*H'+R)
//第五个公式：P = [I-K*H]*P
multiplyMatrix(mykalman->K,NS,NM,mykalman->H,NM,NS,mykalman->temp_5); //temp_5 = K*H
subMatrix(mykalman->E,NS,NS,mykalman->temp_5,NS,NS,mykalman->temp_5_1); //temp_5_1 = E-K*H
multiplyMatrix(mykalman->temp_5_1,NS,NS,mykalman->P,NS,NS,mykalman->temp_5_2); //temp_5_2 = (E-K*H)*P
for (i=0;i<NS;i++)
for (j=0;j<NS;j++)
mykalman->P[i][j] = mykalman->temp_5_2[i][j];
}
void cCamera::Update(float deltaTime)
{
UpdateInput(deltaTime);
esMatrixLoadIdentity( & Camera );
if(m_enabled)
{
cVector2di mousePos=INPUT->getMousePos();
float rotatex=(float)(mousePos.x-m_prevMouseX);
float rotatey=(float)(mousePos.y-m_prevMouseY);
m_currentRot.x+=rotatex;
m_currentRot.z+=rotatey;
m_prevMouseX=mousePos.x;
m_prevMouseY=mousePos.y;
}
esTranslate(&Camera,m_currentPos.x,m_currentPos.y,m_currentPos.z);
esRotate(&Camera,m_currentRot.x,0,1,0);
esRotate(&Camera,m_currentRot.z,1,0,0);
InverseMatrix(Camera,View);
}
OSErr QTTarg_AddTextToggleButtonTrack (Movie theMovie)
{
Track myTrack = NULL;
Media myMedia = NULL;
MatrixRecord myMatrix;
RGBColor myKeyColor;
Fixed myWidth, myHeight;
TimeValue myDuration = 0L;
TimeValue myTimeScale = 0L;
OSErr myErr = noErr;
//////////
//
// get some information about the target movie
//
//////////
if (theMovie == NULL) {
myErr = paramErr;
goto bail;
}
myWidth = Long2Fix(2 * kButtonWidth);
myHeight = Long2Fix(2 * kButtonHeight);
myDuration = GetMovieDuration(theMovie);
myTimeScale = GetMovieTimeScale(theMovie);
//////////
//
// create a new sprite track in the target movie
//
//////////
myTrack = NewMovieTrack(theMovie, myWidth, myHeight, kNoVolume);
myMedia = NewTrackMedia(myTrack, SpriteMediaType, myTimeScale, NULL, 0);
// set the track matrix to compensate for any existing movie matrix
GetMovieMatrix(theMovie, &myMatrix);
if (InverseMatrix(&myMatrix, &myMatrix))
SetTrackMatrix(myTrack, &myMatrix);
myErr = BeginMediaEdits(myMedia);
if (myErr != noErr)
goto bail;
//////////
//
// add sprite images and sprites to the sprite track; add actions to the sprites
//
//////////
QTTarg_AddTextButtonSamplesToMedia(myMedia, 2 * kButtonWidth, 2 * kButtonHeight, myDuration);
//////////
//
// insert media into track
//
//////////
myErr = EndMediaEdits(myMedia);
if (myErr != noErr)
goto bail;
// add the media to the track
InsertMediaIntoTrack(myTrack, 0, 0, GetMediaDuration(myMedia), fixed1);
//////////
//
// set the sprite track properties
//
//////////
QTTarg_SetTrackProperties(myMedia, kNoQTIdleEvents); // no idle events
myKeyColor.red = myKeyColor.green = myKeyColor.blue = 0xffff; // white
MediaSetGraphicsMode(GetMediaHandler(myMedia), transparent, &myKeyColor);
// make sure that the sprite track is in the frontmost layer
SetTrackLayer(myTrack, kMaxLayerNumber);
SetTrackLayer(myTrack, QTTarg_GetLowestLayerInMovie(theMovie) - 1);
bail:
return(myErr);
}
CameraMatrix::CameraMatrix(double* paramaters,double* joints,double x, double y, double bearing, bool bottomCamera)
{
// Initialise the matrix variables
leftCam = Matrix(4,4,true);
rightCam = Matrix(4,4,true);
// Set the constants
const double TORSO_HEIGHT = 21.55,HEAD_DEPTH = 5.39,HEAD_HEIGHT = 6.79;
const double FOOT_HEIGHT = 4.6,TIBIA_LENGTH = 10,THIGH_LENGTH = 10;
const double LEFT_HIP_OFFSET = -5, RIGHT_HIP_OFFSET = 5;
// Declare all the transformation matrices
Matrix lPositionTrans,rPositionTrans,bearingRot,footHeightTrans,tibiaTrans,thighTrans, torsoTrans;
Matrix headYawRot,headPitchRot,headTrans;
Matrix lHipYawPitchRot,lHipRollRot,lHipPitchRot,lKneePitchRot,lAnklePitchRot,lAnkleRollRot,lHipOffsetTrans;
Matrix rHipYawPitchRot,rHipRollRot,rHipPitchRot,rKneePitchRot,rAnklePitchRot,rAnkleRollRot,rHipOffsetTrans;
Matrix headYawParamaterRot,headPitchParamaterRot,lHipPitchParamaterRot,lHipRollParamaterRot,
rHipPitchParamaterRot,rHipRollParamaterRot;
// Set all the transformation matrices
bearingRot = Rot(bearing, 'z');
footHeightTrans = Trans(0,0,FOOT_HEIGHT);
tibiaTrans = Trans(0,0,TIBIA_LENGTH);
thighTrans = Trans(0,0,THIGH_LENGTH);
torsoTrans = Trans(0,0,TORSO_HEIGHT);
headYawRot = Rot(joints[0],'z');
if (bottomCamera)
{
headPitchRot = Rot(joints[1]+40, 'y');
headTrans = Trans(HEAD_DEPTH-0.51,0,HEAD_HEIGHT-4.409);
}
else
{
headPitchRot = Rot(joints[1], 'y');
headTrans = Trans(HEAD_DEPTH,0,HEAD_HEIGHT);
}
lHipYawPitchRot = Rot(joints[10]/2,'z') * Rot(joints[10]/2,'y');
lHipRollRot = Rot(joints[11],'x');
lHipPitchRot = Rot(joints[12],'y');
lKneePitchRot = Rot(joints[13],'y');
lAnklePitchRot = Rot(joints[14],'y');
lAnkleRollRot = Rot(joints[15],'x');
lHipOffsetTrans = Trans(0,LEFT_HIP_OFFSET,0);
rHipYawPitchRot = Rot(-joints[16]/2,'z') * Rot(joints[16]/2,'y');
rHipRollRot = Rot(joints[17],'x');
rHipPitchRot = Rot(joints[18],'y');
rKneePitchRot = Rot(joints[19],'y');
rAnklePitchRot = Rot(joints[20],'y');
rAnkleRollRot = Rot(joints[21],'x');
rHipOffsetTrans = Trans(0,RIGHT_HIP_OFFSET,0);
headYawParamaterRot = Rot(paramaters[0],'z');
headPitchParamaterRot = Rot(paramaters[1],'y');
lHipPitchParamaterRot = Rot(paramaters[2],'y');
lHipRollParamaterRot = Rot(paramaters[3],'x');
rHipPitchParamaterRot = Rot(paramaters[4],'y');
rHipRollParamaterRot = Rot(paramaters[5],'x');
//Find position of each foot by working backwards from hip
Matrix leftLegTemp = Matrix(4,1);
Matrix rightLegTemp = Matrix(4,1);
leftLegTemp[3][0] = 1;
rightLegTemp[3][0] = 1;
leftLegTemp = bearingRot * InverseMatrix(footHeightTrans * lAnkleRollRot * lAnklePitchRot * tibiaTrans * lKneePitchRot *
thighTrans * lHipPitchRot * lHipRollRot * lHipYawPitchRot * lHipOffsetTrans) * leftLegTemp;
rightLegTemp = bearingRot * InverseMatrix(footHeightTrans * rAnkleRollRot * rAnklePitchRot * tibiaTrans * rKneePitchRot *
thighTrans * rHipPitchRot * rHipRollRot * rHipYawPitchRot * rHipOffsetTrans) * rightLegTemp;
// Create both position transformations based on position of feet
lPositionTrans = Trans(x+leftLegTemp[0][0],y+leftLegTemp[1][0],0);
rPositionTrans = Trans(x+rightLegTemp[0][0],y+rightLegTemp[1][0],0);
//Create total leg and torso transformations
Matrix totalLeftLegTransformation = lPositionTrans * bearingRot * footHeightTrans * lAnkleRollRot * lAnklePitchRot *
tibiaTrans * lKneePitchRot * thighTrans * lHipPitchRot * lHipPitchParamaterRot *
lHipRollRot * lHipRollParamaterRot * lHipYawPitchRot * lHipOffsetTrans;
Matrix totalRightLegTransformation = rPositionTrans * bearingRot * footHeightTrans * rAnkleRollRot * rAnklePitchRot *
tibiaTrans * rKneePitchRot * thighTrans * rHipPitchRot * rHipPitchParamaterRot *
rHipRollRot * rHipRollParamaterRot * rHipYawPitchRot * rHipOffsetTrans;
Matrix totalTorsoTransformation = torsoTrans * headYawRot * headYawParamaterRot * headPitchRot * headPitchParamaterRot
* headTrans;
// Create total transformation matrices. Note: inverted to provide World-to-Camera coordinates
leftCam = InverseMatrix(totalLeftLegTransformation * totalTorsoTransformation);
rightCam = InverseMatrix(totalRightLegTransformation * totalTorsoTransformation);
// Create matrix transform axis from robot coordinate system
// to vision coordinate system (NOT screen coordinates)
/*
Robot Coordinate System Vision Coordinate System
z y
| x | z
| / | /
y_____|/ |/_____ x
*/
Matrix axisTransform = Matrix(4,4);
axisTransform[0][1] = -1;
axisTransform[1][2] = 1;
axisTransform[2][0] = 1;
axisTransform[3][3] = 1;
//Multiply camera by axis transformation to get World-to-Vision transformation
leftCam = axisTransform * leftCam;
rightCam = axisTransform * rightCam;
}
//*****************************************************************************
// Calculate Euler Angle with Kalman Filter
//*****************************************************************************
void KalmanFilter(){
float a[3], w[3], L[3], t[4], m_dot[3],q[4];
float norm, inner_q;
float q1[4], q2[4], psi;
float buff_arr[4][4];
int i,j,k,l;
// 1. IMU to Quatornion
GetFromMPU6050(a, 1);
GetFromMPU6050(w, 0);
w[0] -= _gyro_offset[0];
w[1] -= _gyro_offset[1];
w[2] -= _gyro_offset[2];
norm = sqrtf(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
a[0] /= norm;
a[1] /= norm;
a[2] /= norm;
if(a[2] >= 0.0){
q1[0] = sqrtf((1+a[2])/2);
q1[1]= a[1]/sqrtf(2*(1+a[2]));
q1[2] = -a[0]/sqrtf(2*(1+a[2]));
q1[3] = 0;
}else{
q1[0] = a[1]/sqrtf(2*(1-a[2]));
q1[1] = sqrtf((1-a[2])/2);
q1[2] = 0;
q1[3] = a[0]/sqrtf(2*(1-a[2]));
}
// q_mag
m_dot[0] = w[1]*m[2]-w[2]*m[1];
m_dot[1] = w[2]*m[0]-w[0]*m[2];
m_dot[2] = w[0]*m[1]-w[1]*m[0];
m[0] = m[0] - m_dot[0]*dt;
m[1] = m[1] - m_dot[1]*dt;
m[2] = m[2] - m_dot[2]*dt;
norm = sqrtf(m[0]*m[0] + m[1]*m[1] + m[2]*m[2]);
m[0] /= norm;
m[1] /= norm;
m[2] /= norm;
t[0] = -q1[1]*m[0] - q1[2]*m[1] - q1[3]*m[2];
t[1] = q1[0]*m[0] - q1[3]*m[1] + q1[2]*m[2];
t[2] = q1[3]*m[0] + q1[0]*m[1] - q1[1]*m[2];
t[3] = -q1[2]*m[0] + q1[1]*m[1] + q1[0]*m[2];
L[0] = t[1]*q1[0] - t[0]*q1[1] + t[3]*q1[2] - t[2]*q1[3];
L[1] = t[2]*q1[0] - t[3]*q1[1] - t[0]*q1[2] + t[1]*q1[3];
L[2] = t[3]*q1[0] + t[2]*q1[1] - t[1]*q1[2] - t[0]*q1[3];
psi = atan2f(L[1],L[0]);
q2[0] = cosf(psi/2.0);
q2[1] = 0;
q2[2] = 0;
q2[3] = -sinf(psi/2.0);
// q
q[0] = q2[0]*q1[0] - q2[1]*q1[1] - q2[2]*q1[2] - q2[3]*q1[3];
q[1] = q2[1]*q1[0] + q2[0]*q1[1] - q2[3]*q1[2] + q2[2]*q1[3];
q[2] = q2[2]*q1[0] + q2[3]*q1[1] + q2[0]*q1[2] - q2[1]*q1[3];
q[3] = q2[3]*q1[0] - q2[2]*q1[1] + q2[1]*q1[2] + q2[0]*q1[3];
// 2. Remove quatornion discontiunity
inner_q = q_kalman[0]*q[0] + q_kalman[1]*q[1] + q_kalman[2]*q[2] + q_kalman[3]*q[3];
if(inner_q < 0){
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// 3. Kalman Filter
// 3.1. System Variables
A[0][0] = 1; A[0][1] = -dt/2*w[0]; A[0][2] = -dt/2*w[1]; A[0][3] = -dt/2*w[2];
A[1][0] = dt/2*w[0]; A[1][1] = 1; A[1][2] = dt/2*w[2]; A[1][3] = -dt/2*w[1];
A[2][0] = dt/2*w[1]; A[2][1] = -dt/2*w[2]; A[2][2] = 1; A[2][3] = dt/2*w[0];
A[3][0] = dt/2*w[2]; A[3][1] = dt/2*w[1]; A[3][2] = -dt/2*w[0]; A[3][3] = 1;
// 3.2. xp = A*x
xp[0] = A[0][0]*q_kalman[0] + A[0][1]*q_kalman[1] + A[0][2]*q_kalman[2] + A[0][3]*q_kalman[3];
xp[1] = A[1][0]*q_kalman[0] + A[1][1]*q_kalman[1] + A[1][2]*q_kalman[2] + A[1][3]*q_kalman[3];
xp[2] = A[2][0]*q_kalman[0] + A[2][1]*q_kalman[1] + A[2][2]*q_kalman[2] + A[2][3]*q_kalman[3];
xp[3] = A[3][0]*q_kalman[0] + A[3][1]*q_kalman[1] + A[3][2]*q_kalman[2] + A[3][3]*q_kalman[3];
// 3.3. Pp = A*P*A' + Q
for(i=0;i<4;i++){
for(j=0;j<4;j++){
Pp[i][j] = 0;
for(k=0;k<4;k++){
for(l=0;l<4;l++)
Pp[i][j] += A[i][l]*P_kalman[l][k]*A[j][k];
}
if(i==j)
Pp[i][j] += Q_q;
buff_arr[i][j] = Pp[i][j];
}
}
// 3.4. K = Pp*inv(Pp+R)
buff_arr[0][0] += R_r;
buff_arr[1][1] += R_r;
buff_arr[2][2] += R_r;
buff_arr[3][3] += R_r;
InverseMatrix(buff_arr,4);
for(i=0;i<4;i++){
for(j=0;j<4;j++)
K[i][j] = Pp[i][0]*buff_arr[0][j] + Pp[i][1]*buff_arr[1][j] + Pp[i][2]*buff_arr[2][j] + Pp[i][3]*buff_arr[3][j];
}
// 3.5. Update q_kalman = xp + K(q-xp)
for(i=0;i<4;i++){
q_kalman[i] = xp[i];
for(j=0;j<4;j++){
q_kalman[i] += K[i][j]*(q[j]-xp[j]);
}
}
// 3.6.Update P_kalman = Pp - K*Pp
for(i=0;i<4;i++){
for(j=0;j<4;j++){
P_kalman[i][j] = Pp[i][j];
for(k=0;k<4;k++)
P_kalman[i][j] -= K[i][k]*Pp[k][j];
}
}
_quaternion[0] = _q_offset[0]*q_kalman[0] - _q_offset[1]*q_kalman[1] - _q_offset[2]*q_kalman[2] + _q_offset[3]*q_kalman[3];
_quaternion[1] = _q_offset[1]*q_kalman[0] + _q_offset[0]*q_kalman[1] - _q_offset[3]*q_kalman[2] - _q_offset[2]*q_kalman[3];
_quaternion[2] = _q_offset[2]*q_kalman[0] + _q_offset[3]*q_kalman[1] + _q_offset[0]*q_kalman[2] + _q_offset[1]*q_kalman[3];
_quaternion[3] =-_q_offset[3]*q_kalman[0] + _q_offset[2]*q_kalman[1] - _q_offset[1]*q_kalman[2] + _q_offset[0]*q_kalman[3];
// 4. Euler Angle
_euler_angle[0] = atan2f(2.0*(_quaternion[2]*_quaternion[3]+_quaternion[0]*_quaternion[1]), 1.0-2.0*(_quaternion[1]*_quaternion[1]+_quaternion[2]*_quaternion[2]));
_euler_angle[1] = asinf(2.0*(_quaternion[0]*_quaternion[2]-_quaternion[3]*_quaternion[1]));
_euler_angle[2] = atan2f(2.0*(_quaternion[1]*_quaternion[2]+_quaternion[0]*_quaternion[3]), 1.0-2.0*(_quaternion[2]*_quaternion[2]+_quaternion[3]*_quaternion[3]));
_w_gyro[0] = w[0];
_w_gyro[1] = w[1];
_w_gyro[2] = w[2];
}
//Takes a snapshot with the given camera.
//Arguments:
// The Runge-Kutta method to be used.
// The filename.
// Maximum number of steps a light ray may take.
// The maximum distance from the black hole a ray is allowed to go before it is stopped.
// The initial step size.
// Coordinate code specifying the output coordinates to say where the light ray ends up.
// A coordinate test function.
void Camera3D::Snapshot(RungeKuttaForm const *solvedata, char const*file, int maxsteps, double maxdistance, double initstep, double errscale, int outputcoordinate, int (*coordtest)(int, double[], double[]))
{
double error[17];
double *lambda1, *lambda2, *temp, *dlambda1, *dlambda2;
lambda1 = new double[17]; lambda2 = new double[17]; dlambda1 = new double[17]; dlambda2 = new double[17];
double step, s, dist, distmin, distmax, previousdist, *v;
int presentcoordinate, newcoordinate;
ofstream out;
out.open(file);
int i, j, counter = 0, outeradjustments;
double maxdistancesquared = maxdistance*maxdistance;
double stepmax, stepmin;
bool initeval;
double inversedirections[16];
void (*initialderivatives)(double, double[], double[]);
void (*derivatives)(double, double[], double[]);
double (*initialdistance)(double[]);
double (*distance)(double[]);
RungeKuttaData *solver = new RungeKuttaData(solvedata, 8);
//The different coordinate schemes:
//0: past EF coordinates
//1: future EF coordinates
//2: cartesian r > 0.
//3: cartesian r < 0.
switch (coordinate){
case PAST_EF:
initialderivatives = DerivativesPastEddingtonFinkelstein;
initialdistance = DistancePastEddingtonFinkelstein;
break;
case FUTURE_EF:
initialderivatives = DerivativesFutureEddingtonFinkelstein;
initialdistance = DistanceFutureEddingtonFinkelstein;
break;
case CARTESIAN_POSITIVE_R:
initialderivatives = DerivativesCartesian;
initialdistance = DistanceCartesian;
break;
case CARTESIAN_NEGATIVE_R:
initialderivatives = DerivativesCartesian;
initialdistance = DistanceCartesian;
break;
}
InverseMatrix(inversedirections, directions);
out << pixh << " " << pixv << endl;
for(i=0;i<pix;i++)
{
s = 0.0; step = initstep;
dist = 0.0;
for(j = 0; j < 4; j++) lambda1[j] = pos[j];
while(j < 8) {lambda1[j] = light[13*i+j-4];j++;}
for(j = 0; j < 8; j++) lambda2[j] = lambda1[j];
derivatives = initialderivatives;
distance = initialdistance;
initeval = true;
presentcoordinate = coordinate;
for(j=0;j<maxsteps;j++)
{
counter ++;
if(initeval)derivatives(0.0, lambda1, dlambda1);
initeval = false;
step = solver->timestep(derivatives, lambda1, dlambda1, 0.0, step, 1., errscale, lambda2, error, dlambda2);
if (((step >= -.0000001)&&(step <= .0000001)) || isnan(step) || isinf(step)) break;
distmin = dist;
dist = distance(lambda2);
//If the light ray goes outside the max distance, it is necessary to track it
// to find its location at exactly that distance. This loop tests different
// lengths for the last time step until the light ray is as close as possible
// to the max distance. IF this is not done, the picture comes out a little
// funny-looking.
if (dist > maxdistancesquared)
{
outeradjustments = 0;
previousdist = pow(dist, .5);
distmin = pow(distmin, .5);
distmax = previousdist;
stepmax = step; stepmin = 0;
while(true)
{
dist = pow(dist, .5);
if(dist < maxdistance) {
stepmin = step;
distmin = dist;
} else {
if(dist - DISTANCERANGE < maxdistance) break;
else {
stepmax = step; distmax = dist;
}
}
step = stepmin + (stepmax - stepmin)*(maxdistance - distmin)/(distmax - distmin);
solver->step(derivatives, lambda1, dlambda2, 0.0, step, lambda2, error, dlambda2);
dist = distance(lambda2);
if(outeradjustments > MAXOUTERADJUSTMENTS) break;
outeradjustments++;
}
break;
}
v = &lambda1[4];
//Test if a coordinate change is needed.
newcoordinate = coordtest(presentcoordinate, lambda2, lambda2);
if(newcoordinate != presentcoordinate) {
switch (newcoordinate){
case PAST_EF:
derivatives = DerivativesPastEddingtonFinkelstein;
distance = DistancePastEddingtonFinkelstein;
break;
case FUTURE_EF:
derivatives = DerivativesFutureEddingtonFinkelstein;
distance = DistanceFutureEddingtonFinkelstein;
break;
case CARTESIAN_POSITIVE_R:
derivatives = DerivativesCartesian;
distance = DistanceCartesian;
break;
case CARTESIAN_NEGATIVE_R:
derivatives = DerivativesCartesian;
distance = DistanceCartesian;
break;
}
CoordinateSwitch(blackhole, lambda2, v, 1, newcoordinate, presentcoordinate);
initeval = true;
}
presentcoordinate = newcoordinate;
temp = lambda1; lambda1 = lambda2; lambda2 = temp;
temp = dlambda1; dlambda1 = dlambda2; dlambda2 = temp;
}
CoordinateSwitch(blackhole, lambda2, &lambda2[4], 1, 4, presentcoordinate);
}
delete solver;
delete[] lambda1; delete[] lambda2; delete[] dlambda1; delete[] dlambda2;
}
static void MatrixInv(double matrix_in [3][3], double matrix_out [3][3])
{
int userN = 3;
int userM = 2;
int row = 0;
int column = 0;
double** matrixA;
double** inversedMatrixA;
double** multiplicationalMatrixA;
double determinantResult = 0;
matrixA = (double**)malloc(sizeof(double*) * userN);
for (row = 0; row < userN; row++)
{
matrixA[row] = (double*)malloc(sizeof(double) * userN);
} /* for (row = 0; row < userN; row++) */
for (row = 0; row < userN; row++)
{
for (column = 0; column < userN; column++)
{
matrixA[row][column] = matrix_in[row][column];
} /* for (column = 0; column < userN; column++) */
} /* for (row = 0; row < userN; row++) */
determinantResult = Determinant(matrixA, userN);
if (!determinantResult)
{
printf("\nNo determinant(A)\n");
}
else
{
inversedMatrixA = (double**)malloc(sizeof(double*) * userN);
multiplicationalMatrixA = (double**)malloc(sizeof(double*) * userN);
for (row = 0; row < userN; row++)
{
inversedMatrixA[row] = (double*)malloc(sizeof(double) * userN);
multiplicationalMatrixA[row] =
(double*)malloc(sizeof(double) * userN);
}
InverseMatrix(matrixA, inversedMatrixA, userN, determinantResult);
for (row = 0; row < userN; row++)
{
for (column = 0; column < userN; column++)
{
matrix_out[row][column] = inversedMatrixA[row][column];
}
}
/* MultiplicationMatrix(matrixA, inversedMatrixA,
multiplicationalMatrixA, userN);
printf("\nA'A = \n");
for (row = 0; row < userN; row++)
{
for (column = 0; column < userN; column++)
{
printf("%15.5lf ", multiplicationalMatrixA[row][column]);
if (column % userN == userM)
{
printf("\n");
}
}
}
*/
for(row = 0; row < userN; row++)
{
free(multiplicationalMatrixA[row]);
free(inversedMatrixA[row]);
} /* for(row = 0; row < userN; row++) */
free(multiplicationalMatrixA);
free(inversedMatrixA);
}
for(row = 0; row < userN; row++)
{
free(matrixA[row]);
} /* for(row = 0; row < userN; row++) */
free(matrixA);
}
void Arnoldi<SCAL>::Calc (int numval, Array<Complex> & lam, int numev,
Array<shared_ptr<BaseVector>> & hevecs,
const BaseMatrix * pre) const
{
static Timer t("arnoldi");
static Timer t2("arnoldi - orthogonalize");
static Timer t3("arnoldi - compute large vectors");
RegionTimer reg(t);
auto hv = a.CreateVector();
auto hv2 = a.CreateVector();
auto hva = a.CreateVector();
auto hvm = a.CreateVector();
int n = hv.FV<SCAL>().Size();
int m = min2 (numval, n);
Matrix<SCAL> matH(m);
Array<shared_ptr<BaseVector>> abv(m);
for (int i = 0; i < m; i++)
abv[i] = a.CreateVector();
auto mat_shift = a.CreateMatrix();
mat_shift->AsVector() = a.AsVector() - shift*b.AsVector();
shared_ptr<BaseMatrix> inv;
if (!pre)
inv = mat_shift->InverseMatrix (freedofs);
else
{
auto itso = make_shared<GMRESSolver<double>> (*mat_shift, *pre);
itso->SetPrintRates(1);
itso->SetMaxSteps(2000);
inv = itso;
}
hv.SetRandom();
hv.SetParallelStatus (CUMULATED);
FlatVector<SCAL> fv = hv.FV<SCAL>();
if (freedofs)
for (int i = 0; i < hv.Size(); i++)
if (! (*freedofs)[i] ) fv(i) = 0;
t2.Start();
// matV = SCAL(0.0); why ?
matH = SCAL(0.0);
*hv2 = *hv;
SCAL len = sqrt (S_InnerProduct<SCAL> (*hv, *hv2)); // parallel
*hv /= len;
for (int i = 0; i < m; i++)
{
cout << IM(1) << "\ri = " << i << "/" << m << flush;
/*
for (int j = 0; j < n; j++)
matV(i,j) = hv.FV<SCAL>()(j);
*/
*abv[i] = *hv;
*hva = b * *hv;
*hvm = *inv * *hva;
for (int j = 0; j <= i; j++)
{
/*
SCAL sum = 0.0;
for (int k = 0; k < n; k++)
sum += hvm.FV<SCAL>()(k) * matV(j,k);
matH(j,i) = sum;
for (int k = 0; k < n; k++)
hvm.FV<SCAL>()(k) -= sum * matV(j,k);
*/
/*
SCAL sum = 0.0;
FlatVector<SCAL> abvj = abv[j] -> FV<SCAL>();
FlatVector<SCAL> fv_hvm = hvm.FV<SCAL>();
for (int k = 0; k < n; k++)
sum += fv_hvm(k) * abvj(k);
matH(j,i) = sum;
for (int k = 0; k < n; k++)
fv_hvm(k) -= sum * abvj(k);
*/
matH(j,i) = S_InnerProduct<SCAL> (*hvm, *abv[j]);
*hvm -= matH(j,i) * *abv[j];
}
*hv = *hvm;
*hv2 = *hv;
SCAL len = sqrt (S_InnerProduct<SCAL> (*hv, *hv2));
if (i<m-1) matH(i+1,i) = len;
*hv /= len;
}
t2.Stop();
t2.AddFlops (double(n)*m*m);
cout << "n = " << n << ", m = " << m << " n*m*m = " << n*m*m << endl;
cout << IM(1) << "\ri = " << m << "/" << m << endl;
Vector<Complex> lami(m);
Matrix<Complex> evecs(m);
Matrix<Complex> matHt(m);
matHt = Trans (matH);
evecs = Complex (0.0);
lami = Complex (0.0);
cout << "Solve Hessenberg evp with Lapack ... " << flush;
LapackHessenbergEP (matH.Height(), &matHt(0,0), &lami(0), &evecs(0,0));
cout << "done" << endl;
for (int i = 0; i < m; i++)
lami(i) = 1.0 / lami(i) + shift;
lam.SetSize (m);
for (int i = 0; i < m; i++)
lam[i] = lami(i);
t3.Start();
if (numev>0)
{
int nout = min2 (numev, m);
hevecs.SetSize(nout);
for (int i = 0; i< nout; i++)
{
hevecs[i] = a.CreateVector();
*hevecs[i] = 0;
for (int j = 0; j < m; j++)
*hevecs[i] += evecs(i,j) * *abv[j];
// hevecs[i]->FVComplex() = Trans(matV)*evecs.Row(i);
}
}
t3.Stop();
}
void RGBColorSystem::Data::Initialize()
{
/*
* Normalize luminance coefficients
*/
double s = Y.Sum();
if ( 1 + s == 1 )
throw Error( "Invalid luminance coefficients in RGB working color space initialization." );
Y /= s;
/*
* RGB -> XYZ transformation matrix M
*/
M = SetupMatrix( x, y, Y );
/*
* CIE X and CIE Z normalization coefficients
*/
mX = M11 + M12 + M13;
mZ = M31 + M32 + M33;
/*
* XYZ -> RGB inverse matrix M_
*/
M_ = InverseMatrix( M );
/*
* Inverse gamma
*/
if ( 1 + gamma == 1 || gamma < 0 )
throw Error( "Invalid gamma value in RGB working color space initialization." );
gammaInv = 1/gamma;
/*
* Find normalization coefficients for CIE a, b, c channels
*
* The idea here is to maximize dynamic range usage for each channel
* (coding efficiency) while ensuring that they will be constrained to the
* nominal range [0,1].
*/
sample minab = 100, maxab = -100, maxc = -100;
int minabR = 0, minabG = 0, minabB = 0;
int maxabR = 0, maxabG = 0, maxabB = 0;
int maxcR = 0, maxcG = 0, maxcB = 0;
for ( int ri = 0; ri < 10; ++ri )
{
sample R = sample( ri/9.0 );
for ( int gi = 0; gi < 10; ++gi )
{
sample G = sample( gi/9.0 );
for ( int bi = 0; bi < 10; ++bi )
{
sample B = sample( bi/9.0 );
sample X, Y, Z;
RGBToCIEXYZ( X, Y, Z, R, G, B );
RGBColorSystem::XYZLab( X );
RGBColorSystem::XYZLab( Y );
RGBColorSystem::XYZLab( Z );
sample a = 5*(X - Y);
sample b = 2*(Y - Z);
sample c = Sqrt( a*a + b*b );
sample mn = Min( a, b );
sample mx = Max( a, b );
if ( mn < minab )
{
minab = mn;
minabR = ri;
minabG = gi;
minabB = bi;
}
if ( mx > maxab )
{
maxab = mx;
maxabR = ri;
maxabG = gi;
maxabB = bi;
}
if ( c > maxc )
{
maxc = c;
maxcR = ri;
maxcG = gi;
maxcB = bi;
}
}
}
}
sample R0, R1, G0, G1, B0, B1;
R0 = Max( 0, minabR-1 )/9.0;
R1 = Min( 9, minabR+1 )/9.0;
G0 = Max( 0, minabG-1 )/9.0;
G1 = Min( 9, minabG+1 )/9.0;
B0 = Max( 0, minabB-1 )/9.0;
B1 = Min( 9, minabB+1 )/9.0;
for ( sample R = R0; R < R1; R += 0.01 )
for ( sample G = G0; G < G1; G += 0.01 )
for ( sample B = B0; B < B1; B += 0.01 )
{
sample X, Y, Z;
RGBToCIEXYZ( X, Y, Z, R, G, B );
RGBColorSystem::XYZLab( X );
RGBColorSystem::XYZLab( Y );
RGBColorSystem::XYZLab( Z );
sample mn = Min( 5*(X - Y), 2*(Y - Z) );
if ( mn < minab )
minab = mn;
}
R0 = Max( 0, maxabR-1 )/9.0;
R1 = Min( 9, maxabR+1 )/9.0;
G0 = Max( 0, maxabG-1 )/9.0;
G1 = Min( 9, maxabG+1 )/9.0;
B0 = Max( 0, maxabB-1 )/9.0;
B1 = Min( 9, maxabB+1 )/9.0;
for ( sample R = R0; R < R1; R += 0.01 )
for ( sample G = G0; G < G1; G += 0.01 )
for ( sample B = B0; B < B1; B += 0.01 )
{
sample X, Y, Z;
RGBToCIEXYZ( X, Y, Z, R, G, B );
RGBColorSystem::XYZLab( X );
RGBColorSystem::XYZLab( Y );
RGBColorSystem::XYZLab( Z );
sample mx = Max( 5*(X - Y), 2*(Y - Z) );
if ( mx > maxab )
maxab = mx;
}
R0 = Max( 0, maxcR-1 )/9.0;
R1 = Min( 9, maxcR+1 )/9.0;
G0 = Max( 0, maxcG-1 )/9.0;
G1 = Min( 9, maxcG+1 )/9.0;
B0 = Max( 0, maxcB-1 )/9.0;
B1 = Min( 9, maxcB+1 )/9.0;
for ( sample R = R0; R < R1; R += 0.01 )
for ( sample G = G0; G < G1; G += 0.01 )
for ( sample B = B0; B < B1; B += 0.01 )
{
sample X, Y, Z;
RGBToCIEXYZ( X, Y, Z, R, G, B );
RGBColorSystem::XYZLab( X );
RGBColorSystem::XYZLab( Y );
RGBColorSystem::XYZLab( Z );
sample a = 5*(X - Y);
sample b = 2*(Y - Z);
sample c = Sqrt( a*a + b*b );
if ( c > maxc )
maxc = c;
}
abOffset = -minab + 0.05;
abDelta = maxab - minab + 0.1;
cDelta = maxc + 0.05;
}