diff --git a/.svn/entries b/.svn/entries
deleted file mode 100644
index 48082f72f087ce7e6fa75b9c41d7387daecd447b..0000000000000000000000000000000000000000
--- a/.svn/entries
+++ /dev/null
@@ -1 +0,0 @@
-12
diff --git a/.svn/format b/.svn/format
deleted file mode 100644
index 48082f72f087ce7e6fa75b9c41d7387daecd447b..0000000000000000000000000000000000000000
--- a/.svn/format
+++ /dev/null
@@ -1 +0,0 @@
-12
diff --git a/.svn/pristine/01/01916e317b2d5c9d0bf60312e2b458b12fe3ec61.svn-base b/.svn/pristine/01/01916e317b2d5c9d0bf60312e2b458b12fe3ec61.svn-base
deleted file mode 100644
index 69c06f817a76017cfdc1b685fb53172ffb3c60d6..0000000000000000000000000000000000000000
--- a/.svn/pristine/01/01916e317b2d5c9d0bf60312e2b458b12fe3ec61.svn-base
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
- * rtGetInf.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RTGETINF_H__
-#define __RTGETINF_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetInf(void);
-extern real32_T rtGetInfF(void);
-extern real_T rtGetMinusInf(void);
-extern real32_T rtGetMinusInfF(void);
-
-#endif
-/* End of code generation (rtGetInf.h) */
diff --git a/.svn/pristine/01/01e8927fa3dca4f3a95c9d07afa24b36a15e3da6.svn-base b/.svn/pristine/01/01e8927fa3dca4f3a95c9d07afa24b36a15e3da6.svn-base
deleted file mode 100644
index 02bb3fe359b4d44c9d5a72e0ae7c1001910c2241..0000000000000000000000000000000000000000
--- a/.svn/pristine/01/01e8927fa3dca4f3a95c9d07afa24b36a15e3da6.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_H__
-#define __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void Optimal_affine_tracking_3d16_fast_realtime(const real_T pty[4], const real_T ptx[4], real_T p[12]);
-#endif
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime.h) */
diff --git a/.svn/pristine/02/026e658b46833cb3a537f165daf7c9fdc0fe41aa.svn-base b/.svn/pristine/02/026e658b46833cb3a537f165daf7c9fdc0fe41aa.svn-base
deleted file mode 100644
index 56bc1ec6756ddaf322df527c39d31ee70fb6c805..0000000000000000000000000000000000000000
--- a/.svn/pristine/02/026e658b46833cb3a537f165daf7c9fdc0fe41aa.svn-base
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * rtGetNaN.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/*
- * Abstract:
- * MATLAB for code generation function to initialize non-finite, NaN
- */
-#include "rtGetNaN.h"
-#define NumBitsPerChar 8U
-
-/* Function: rtGetNaN ==================================================
- * Abstract:
- * Initialize rtNaN needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetNaN(void)
-{
- size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
- real_T nan = 0.0;
- if (bitsPerReal == 32U) {
- nan = rtGetNaNF();
- } else {
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- union {
- LittleEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0xFFF80000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- nan = tmpVal.fltVal;
- break;
- }
-
- case BigEndian:
- {
- union {
- BigEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
- tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
- nan = tmpVal.fltVal;
- break;
- }
- }
- }
-
- return nan;
-}
-
-/* Function: rtGetNaNF ==================================================
- * Abstract:
- * Initialize rtNaNF needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetNaNF(void)
-{
- IEEESingle nanF = { { 0 } };
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- nanF.wordL.wordLuint = 0xFFC00000U;
- break;
- }
-
- case BigEndian:
- {
- nanF.wordL.wordLuint = 0x7FFFFFFFU;
- break;
- }
- }
-
- return nanF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetNaN.cpp) */
diff --git a/.svn/pristine/0a/0a9c657a0e30c25f7f111bca8e72e7822e3ab1c2.svn-base b/.svn/pristine/0a/0a9c657a0e30c25f7f111bca8e72e7822e3ab1c2.svn-base
deleted file mode 100644
index 99fa51c751a2fcd961f6e556dadf362a09645802..0000000000000000000000000000000000000000
--- a/.svn/pristine/0a/0a9c657a0e30c25f7f111bca8e72e7822e3ab1c2.svn-base
+++ /dev/null
@@ -1,22 +0,0 @@
-<package>
- <description brief="iri_depth_tracker">
-
- iri_depth_tracker
-
- </description>
- <author>shusain</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/iri_depth_tracker</url>
- <depend package="std_msgs"/>
- <depend package="rospy"/>
- <depend package="roscpp"/>
- <depend package="pcl_ros"/>
- <depend package="sensor_msgs"/>
- <depend package="openni_camera"/>
- <depend package="opencv2"/>
- <depend package="cv_bridge"/>
-
-</package>
-
-
diff --git a/.svn/pristine/0b/0b5a2ad59fc732434fc5c3673a75e2cd9a5f6a24.svn-base b/.svn/pristine/0b/0b5a2ad59fc732434fc5c3673a75e2cd9a5f6a24.svn-base
deleted file mode 100644
index 4a065dcc604b04ce306800466fe566fe4aa221ad..0000000000000000000000000000000000000000
--- a/.svn/pristine/0b/0b5a2ad59fc732434fc5c3673a75e2cd9a5f6a24.svn-base
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * rtGetNaN.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RTGETNAN_H__
-#define __RTGETNAN_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetNaN(void);
-extern real32_T rtGetNaNF(void);
-
-#endif
-/* End of code generation (rtGetNaN.h) */
diff --git a/.svn/pristine/10/109b44a55bd31b137e51e730cd7188ab908d80f9.svn-base b/.svn/pristine/10/109b44a55bd31b137e51e730cd7188ab908d80f9.svn-base
deleted file mode 100644
index 324afeed97280d2bf6390a0ec2a709dd2bb2d1b1..0000000000000000000000000000000000000000
--- a/.svn/pristine/10/109b44a55bd31b137e51e730cd7188ab908d80f9.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_rtwutil'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_RTWUTIL_H__
-#define __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_RTWUTIL_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real_T rt_hypotd_snf(real_T u0, real_T u1);
-#endif
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h) */
diff --git a/.svn/pristine/13/131763b6af4a6c040ecc6be52d0a6d111923eacc.svn-base b/.svn/pristine/13/131763b6af4a6c040ecc6be52d0a6d111923eacc.svn-base
deleted file mode 100644
index 2c99d3320722570f81f10fd618c36b8cae94ca0f..0000000000000000000000000000000000000000
--- a/.svn/pristine/13/131763b6af4a6c040ecc6be52d0a6d111923eacc.svn-base
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * mean.h
- *
- * Code generation for function 'mean'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __MEAN_H__
-#define __MEAN_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_mean(const real_T x[50], real_T y[2]);
-extern void c_mean(const real_T x[3888], real_T y[3]);
-extern real_T mean(const real_T x_data[1296], const int32_T x_size[1]);
-#endif
-/* End of code generation (mean.h) */
diff --git a/.svn/pristine/14/14f6c159607ca491fc1c8adb68044c9f6e8d68e4.svn-base b/.svn/pristine/14/14f6c159607ca491fc1c8adb68044c9f6e8d68e4.svn-base
deleted file mode 100644
index 79614d594318f953458a9ad4c0f846e6bfef195d..0000000000000000000000000000000000000000
--- a/.svn/pristine/14/14f6c159607ca491fc1c8adb68044c9f6e8d68e4.svn-base
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * mean.cpp
- *
- * Code generation for function 'mean'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "mean.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_mean(const real_T x[50], real_T y[2])
-{
- int32_T iy;
- int32_T ixstart;
- int32_T j;
- int32_T ix;
- real_T s;
- int32_T k;
- iy = -1;
- ixstart = -1;
- for (j = 0; j < 2; j++) {
- ixstart++;
- ix = ixstart;
- s = x[ixstart];
- for (k = 0; k < 24; k++) {
- ix += 2;
- s += x[ix];
- }
-
- iy++;
- y[iy] = s;
- }
-
- for (iy = 0; iy < 2; iy++) {
- y[iy] /= 25.0;
- }
-}
-
-void c_mean(const real_T x[3888], real_T y[3])
-{
- int32_T ix;
- int32_T iy;
- int32_T i;
- int32_T ixstart;
- real_T s;
- ix = -1;
- iy = -1;
- for (i = 0; i < 3; i++) {
- ixstart = ix + 1;
- ix++;
- s = x[ixstart];
- for (ixstart = 0; ixstart < 1295; ixstart++) {
- ix++;
- s += x[ix];
- }
-
- iy++;
- y[iy] = s;
- }
-
- for (ixstart = 0; ixstart < 3; ixstart++) {
- y[ixstart] /= 1296.0;
- }
-}
-
-real_T mean(const real_T x_data[1296], const int32_T x_size[1])
-{
- real_T y;
- int32_T k;
- if (x_size[0] == 0) {
- y = 0.0;
- } else {
- y = x_data[0];
- for (k = 2; k <= x_size[0]; k++) {
- y += x_data[k - 1];
- }
- }
-
- y /= (real_T)x_size[0];
- return y;
-}
-
-/* End of code generation (mean.cpp) */
diff --git a/.svn/pristine/16/167f1d3c43a2cfc6e986153883d57f83ec4ea7b3.svn-base b/.svn/pristine/16/167f1d3c43a2cfc6e986153883d57f83ec4ea7b3.svn-base
deleted file mode 100644
index c236a3cdf1adacbb7e50eb6f53398779fe0cf9a0..0000000000000000000000000000000000000000
--- a/.svn/pristine/16/167f1d3c43a2cfc6e986153883d57f83ec4ea7b3.svn-base
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * sum.h
- *
- * Code generation for function 'sum'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __SUM_H__
-#define __SUM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_sum(const real_T x[7776], real_T y[6]);
-extern real_T c_sum(const real_T x[25]);
-extern void sum(const real_T x[3888], real_T y[3]);
-#endif
-/* End of code generation (sum.h) */
diff --git a/.svn/pristine/1b/1bac313be96fde1deb7d0d4494cd4e84559c4b52.svn-base b/.svn/pristine/1b/1bac313be96fde1deb7d0d4494cd4e84559c4b52.svn-base
deleted file mode 100644
index a40f61072eba1d919158613905eb9d8b69bef700..0000000000000000000000000000000000000000
--- a/.svn/pristine/1b/1bac313be96fde1deb7d0d4494cd4e84559c4b52.svn-base
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * eye.cpp
- *
- * Code generation for function 'eye'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "eye.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_eye(real_T I[9])
-{
- int32_T i;
- memset(&I[0], 0, 9U * sizeof(real_T));
- for (i = 0; i < 3; i++) {
- I[i + 3 * i] = 1.0;
- }
-}
-
-void eye(real_T I[9])
-{
- int32_T i;
- memset(&I[0], 0, 9U * sizeof(real_T));
- for (i = 0; i < 3; i++) {
- I[i + 3 * i] = 1.0;
- }
-}
-
-/* End of code generation (eye.cpp) */
diff --git a/.svn/pristine/1b/1bb4c9d470acd9ee06afb7017a591d8e10c0cd59.svn-base b/.svn/pristine/1b/1bb4c9d470acd9ee06afb7017a591d8e10c0cd59.svn-base
deleted file mode 100644
index 0dbe2665317f1190e8df09dc5f3af56a4409dd2a..0000000000000000000000000000000000000000
--- a/.svn/pristine/1b/1bb4c9d470acd9ee06afb7017a591d8e10c0cd59.svn-base
+++ /dev/null
@@ -1,617 +0,0 @@
-/*
- * mrdivide.cpp
- *
- * Code generation for function 'mrdivide'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "mrdivide.h"
-#include "log.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real_T b_eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0);
-static real_T eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0);
-
-/* Function Definitions */
-static real_T b_eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0)
-{
- real_T y;
- real_T scale;
- int32_T kend;
- int32_T k;
- real_T absxk;
- real_T t;
- y = 0.0;
- if (n == 1) {
- y = fabs(x[ix0 - 1]);
- } else {
- scale = 2.2250738585072014E-308;
- kend = (ix0 + n) - 1;
- for (k = ix0; k <= kend; k++) {
- absxk = fabs(x[k - 1]);
- if (absxk > scale) {
- t = scale / absxk;
- y = 1.0 + y * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- y += t * t;
- }
- }
-
- y = scale * sqrt(y);
- }
-
- return y;
-}
-
-static real_T eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0)
-{
- real_T y;
- real_T scale;
- int32_T kend;
- int32_T k;
- real_T absxk;
- real_T t;
- y = 0.0;
- if (n == 1) {
- y = fabs(x[ix0 - 1]);
- } else {
- scale = 2.2250738585072014E-308;
- kend = (ix0 + n) - 1;
- for (k = ix0; k <= kend; k++) {
- absxk = fabs(x[k - 1]);
- if (absxk > scale) {
- t = scale / absxk;
- y = 1.0 + y * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- y += t * t;
- }
- }
-
- y = scale * sqrt(y);
- }
-
- return y;
-}
-
-void b_mrdivide(const creal_T A[9], const creal_T B[9], creal_T y[9])
-{
- creal_T b_A[9];
- creal_T b_B[9];
- int32_T rtemp;
- int32_T k;
- int32_T r1;
- int32_T r2;
- int32_T r3;
- real_T maxval;
- real_T a21;
- creal_T Y[9];
- for (rtemp = 0; rtemp < 3; rtemp++) {
- for (k = 0; k < 3; k++) {
- b_A[k + 3 * rtemp].re = B[rtemp + 3 * k].re;
- b_A[k + 3 * rtemp].im = -B[rtemp + 3 * k].im;
- b_B[k + 3 * rtemp].re = A[rtemp + 3 * k].re;
- b_B[k + 3 * rtemp].im = -A[rtemp + 3 * k].im;
- }
- }
-
- r1 = 0;
- r2 = 1;
- r3 = 2;
- maxval = fabs(b_A[0].re) + fabs(b_A[0].im);
- a21 = fabs(b_A[1].re) + fabs(b_A[1].im);
- if (a21 > maxval) {
- maxval = a21;
- r1 = 1;
- r2 = 0;
- }
-
- if (fabs(b_A[2].re) + fabs(b_A[2].im) > maxval) {
- r1 = 2;
- r2 = 1;
- r3 = 0;
- }
-
- b_A[r2] = c_eml_div(b_A[r2], b_A[r1]);
- b_A[r3] = c_eml_div(b_A[r3], b_A[r1]);
- maxval = b_A[r2].re * b_A[3 + r1].im + b_A[r2].im * b_A[3 + r1].re;
- b_A[3 + r2].re -= b_A[r2].re * b_A[3 + r1].re - b_A[r2].im * b_A[3 + r1].im;
- b_A[3 + r2].im -= maxval;
- maxval = b_A[r3].re * b_A[3 + r1].im + b_A[r3].im * b_A[3 + r1].re;
- b_A[3 + r3].re -= b_A[r3].re * b_A[3 + r1].re - b_A[r3].im * b_A[3 + r1].im;
- b_A[3 + r3].im -= maxval;
- maxval = b_A[r2].re * b_A[6 + r1].im + b_A[r2].im * b_A[6 + r1].re;
- b_A[6 + r2].re -= b_A[r2].re * b_A[6 + r1].re - b_A[r2].im * b_A[6 + r1].im;
- b_A[6 + r2].im -= maxval;
- maxval = b_A[r3].re * b_A[6 + r1].im + b_A[r3].im * b_A[6 + r1].re;
- b_A[6 + r3].re -= b_A[r3].re * b_A[6 + r1].re - b_A[r3].im * b_A[6 + r1].im;
- b_A[6 + r3].im -= maxval;
- if (fabs(b_A[3 + r3].re) + fabs(b_A[3 + r3].im) > fabs(b_A[3 + r2].re) + fabs
- (b_A[3 + r2].im)) {
- rtemp = r2;
- r2 = r3;
- r3 = rtemp;
- }
-
- b_A[3 + r3] = c_eml_div(b_A[3 + r3], b_A[3 + r2]);
- maxval = b_A[3 + r3].re * b_A[6 + r2].im + b_A[3 + r3].im * b_A[6 + r2].re;
- b_A[6 + r3].re -= b_A[3 + r3].re * b_A[6 + r2].re - b_A[3 + r3].im * b_A[6 +
- r2].im;
- b_A[6 + r3].im -= maxval;
- for (k = 0; k < 3; k++) {
- Y[3 * k] = b_B[r1 + 3 * k];
- maxval = Y[3 * k].re * b_A[r2].im + Y[3 * k].im * b_A[r2].re;
- Y[1 + 3 * k].re = b_B[r2 + 3 * k].re - (Y[3 * k].re * b_A[r2].re - Y[3 * k].
- im * b_A[r2].im);
- Y[1 + 3 * k].im = b_B[r2 + 3 * k].im - maxval;
- maxval = Y[3 * k].re * b_A[r3].im + Y[3 * k].im * b_A[r3].re;
- a21 = Y[1 + 3 * k].re * b_A[3 + r3].im + Y[1 + 3 * k].im * b_A[3 + r3].re;
- Y[2 + 3 * k].re = (b_B[r3 + 3 * k].re - (Y[3 * k].re * b_A[r3].re - Y[3 * k]
- .im * b_A[r3].im)) - (Y[1 + 3 * k].re * b_A[3 + r3].re - Y[1 + 3 * k].im *
- b_A[3 + r3].im);
- Y[2 + 3 * k].im = (b_B[r3 + 3 * k].im - maxval) - a21;
- Y[2 + 3 * k] = c_eml_div(Y[2 + 3 * k], b_A[6 + r3]);
- maxval = Y[2 + 3 * k].re * b_A[6 + r1].im + Y[2 + 3 * k].im * b_A[6 + r1].re;
- Y[3 * k].re -= Y[2 + 3 * k].re * b_A[6 + r1].re - Y[2 + 3 * k].im * b_A[6 +
- r1].im;
- Y[3 * k].im -= maxval;
- maxval = Y[2 + 3 * k].re * b_A[6 + r2].im + Y[2 + 3 * k].im * b_A[6 + r2].re;
- Y[1 + 3 * k].re -= Y[2 + 3 * k].re * b_A[6 + r2].re - Y[2 + 3 * k].im * b_A
- [6 + r2].im;
- Y[1 + 3 * k].im -= maxval;
- Y[1 + 3 * k] = c_eml_div(Y[1 + 3 * k], b_A[3 + r2]);
- maxval = Y[1 + 3 * k].re * b_A[3 + r1].im + Y[1 + 3 * k].im * b_A[3 + r1].re;
- Y[3 * k].re -= Y[1 + 3 * k].re * b_A[3 + r1].re - Y[1 + 3 * k].im * b_A[3 +
- r1].im;
- Y[3 * k].im -= maxval;
- Y[3 * k] = c_eml_div(Y[3 * k], b_A[r1]);
- }
-
- for (rtemp = 0; rtemp < 3; rtemp++) {
- for (k = 0; k < 3; k++) {
- y[k + 3 * rtemp].re = Y[rtemp + 3 * k].re;
- y[k + 3 * rtemp].im = -Y[rtemp + 3 * k].im;
- }
- }
-}
-
-creal_T c_eml_div(const creal_T x, const creal_T y)
-{
- creal_T z;
- real_T brm;
- real_T bim;
- real_T d;
- if (y.im == 0.0) {
- if (x.im == 0.0) {
- z.re = x.re / y.re;
- z.im = 0.0;
- } else if (x.re == 0.0) {
- z.re = 0.0;
- z.im = x.im / y.re;
- } else {
- z.re = x.re / y.re;
- z.im = x.im / y.re;
- }
- } else if (y.re == 0.0) {
- if (x.re == 0.0) {
- z.re = x.im / y.im;
- z.im = 0.0;
- } else if (x.im == 0.0) {
- z.re = 0.0;
- z.im = -(x.re / y.im);
- } else {
- z.re = x.im / y.im;
- z.im = -(x.re / y.im);
- }
- } else {
- brm = fabs(y.re);
- bim = fabs(y.im);
- if (brm > bim) {
- bim = y.im / y.re;
- d = y.re + bim * y.im;
- z.re = (x.re + bim * x.im) / d;
- z.im = (x.im - bim * x.re) / d;
- } else if (bim == brm) {
- if (y.re > 0.0) {
- bim = 0.5;
- } else {
- bim = -0.5;
- }
-
- if (y.im > 0.0) {
- d = 0.5;
- } else {
- d = -0.5;
- }
-
- z.re = (x.re * bim + x.im * d) / brm;
- z.im = (x.im * bim - x.re * d) / brm;
- } else {
- bim = y.re / y.im;
- d = y.im + bim * y.re;
- z.re = (bim * x.re + x.im) / d;
- z.im = (bim * x.im - x.re) / d;
- }
- }
-
- return z;
-}
-
-void c_mrdivide(const creal_T A[4], const creal_T B[4], creal_T y[4])
-{
- creal_T b_A[4];
- creal_T b_B[4];
- int32_T r1;
- int32_T r2;
- creal_T a21;
- creal_T a22;
- creal_T Y[4];
- int32_T k;
- creal_T c_B;
- creal_T d_B;
- for (r1 = 0; r1 < 2; r1++) {
- for (r2 = 0; r2 < 2; r2++) {
- b_A[r2 + (r1 << 1)].re = B[r1 + (r2 << 1)].re;
- b_A[r2 + (r1 << 1)].im = -B[r1 + (r2 << 1)].im;
- b_B[r2 + (r1 << 1)].re = A[r1 + (r2 << 1)].re;
- b_B[r2 + (r1 << 1)].im = -A[r1 + (r2 << 1)].im;
- }
- }
-
- if (fabs(b_A[1].re) + fabs(b_A[1].im) > fabs(b_A[0].re) + fabs(b_A[0].im)) {
- r1 = 1;
- r2 = 0;
- } else {
- r1 = 0;
- r2 = 1;
- }
-
- a21 = c_eml_div(b_A[r2], b_A[r1]);
- a22.re = b_A[2 + r2].re - (a21.re * b_A[2 + r1].re - a21.im * b_A[2 + r1].im);
- a22.im = b_A[2 + r2].im - (a21.re * b_A[2 + r1].im + a21.im * b_A[2 + r1].re);
- for (k = 0; k < 2; k++) {
- c_B.re = b_B[r2 + (k << 1)].re - (b_B[r1 + (k << 1)].re * a21.re - b_B[r1 +
- (k << 1)].im * a21.im);
- c_B.im = b_B[r2 + (k << 1)].im - (b_B[r1 + (k << 1)].re * a21.im + b_B[r1 +
- (k << 1)].im * a21.re);
- Y[1 + (k << 1)] = c_eml_div(c_B, a22);
- d_B.re = b_B[r1 + (k << 1)].re - (Y[1 + (k << 1)].re * b_A[2 + r1].re - Y[1
- + (k << 1)].im * b_A[2 + r1].im);
- d_B.im = b_B[r1 + (k << 1)].im - (Y[1 + (k << 1)].re * b_A[2 + r1].im + Y[1
- + (k << 1)].im * b_A[2 + r1].re);
- Y[k << 1] = c_eml_div(d_B, b_A[r1]);
- }
-
- for (r1 = 0; r1 < 2; r1++) {
- for (r2 = 0; r2 < 2; r2++) {
- y[r2 + (r1 << 1)].re = Y[r1 + (r2 << 1)].re;
- y[r2 + (r1 << 1)].im = -Y[r1 + (r2 << 1)].im;
- }
- }
-}
-
-void mrdivide(const real_T A[12], const real_T B[12], real_T y[9])
-{
- real_T tau[3];
- real_T b_A[12];
- real_T b_B[12];
- int8_T jpvt[3];
- real_T work[3];
- int32_T i4;
- int32_T itemp;
- real_T vn1[3];
- real_T vn2[3];
- int32_T k;
- int32_T iy;
- real_T b_y;
- real_T wj;
- real_T rankR;
- real_T t;
- int32_T i;
- int32_T i_i;
- int32_T ix;
- int32_T pvt;
- int32_T i_ip1;
- int32_T lastv;
- int32_T lastc;
- boolean_T exitg2;
- int32_T exitg1;
- real_T Y[9];
- for (i4 = 0; i4 < 3; i4++) {
- for (itemp = 0; itemp < 4; itemp++) {
- b_A[itemp + (i4 << 2)] = B[i4 + 3 * itemp];
- b_B[itemp + (i4 << 2)] = A[i4 + 3 * itemp];
- }
-
- jpvt[i4] = (int8_T)(1 + i4);
- work[i4] = 0.0;
- }
-
- k = 1;
- for (iy = 0; iy < 3; iy++) {
- b_y = 0.0;
- wj = 2.2250738585072014E-308;
- for (itemp = k; itemp <= k + 3; itemp++) {
- rankR = fabs(b_A[itemp - 1]);
- if (rankR > wj) {
- t = wj / rankR;
- b_y = 1.0 + b_y * t * t;
- wj = rankR;
- } else {
- t = rankR / wj;
- b_y += t * t;
- }
- }
-
- b_y = wj * sqrt(b_y);
- vn1[iy] = b_y;
- vn2[iy] = vn1[iy];
- k += 4;
- }
-
- for (i = 0; i < 3; i++) {
- i_i = i + (i << 2);
- itemp = 0;
- if (3 - i > 1) {
- ix = i;
- wj = fabs(vn1[i]);
- for (k = 1; k + 1 <= 3 - i; k++) {
- ix++;
- rankR = fabs(vn1[ix]);
- if (rankR > wj) {
- itemp = k;
- wj = rankR;
- }
- }
- }
-
- pvt = i + itemp;
- if (pvt + 1 != i + 1) {
- ix = pvt << 2;
- iy = i << 2;
- for (k = 0; k < 4; k++) {
- wj = b_A[ix];
- b_A[ix] = b_A[iy];
- b_A[iy] = wj;
- ix++;
- iy++;
- }
-
- itemp = jpvt[pvt];
- jpvt[pvt] = jpvt[i];
- jpvt[i] = (int8_T)itemp;
- vn1[pvt] = vn1[i];
- vn2[pvt] = vn2[i];
- }
-
- t = b_A[i_i];
- rankR = 0.0;
- wj = eml_xnrm2(3 - i, b_A, i_i + 2);
- if (wj != 0.0) {
- wj = rt_hypotd_snf(fabs(b_A[i_i]), fabs(wj));
- if (b_A[i_i] >= 0.0) {
- wj = -wj;
- }
-
- if (fabs(wj) < 1.0020841800044864E-292) {
- itemp = 0;
- do {
- itemp++;
- i4 = i_i - i;
- for (k = i_i + 1; k + 1 <= i4 + 4; k++) {
- b_A[k] *= 9.9792015476736E+291;
- }
-
- wj *= 9.9792015476736E+291;
- t *= 9.9792015476736E+291;
- } while (!(fabs(wj) >= 1.0020841800044864E-292));
-
- wj = rt_hypotd_snf(fabs(t), fabs(eml_xnrm2(3 - i, b_A, i_i + 2)));
- if (t >= 0.0) {
- wj = -wj;
- }
-
- rankR = (wj - t) / wj;
- t = 1.0 / (t - wj);
- i4 = i_i - i;
- for (k = i_i + 1; k + 1 <= i4 + 4; k++) {
- b_A[k] *= t;
- }
-
- for (k = 1; k <= itemp; k++) {
- wj *= 1.0020841800044864E-292;
- }
-
- t = wj;
- } else {
- rankR = (wj - b_A[i_i]) / wj;
- t = 1.0 / (b_A[i_i] - wj);
- i4 = i_i - i;
- for (k = i_i + 1; k + 1 <= i4 + 4; k++) {
- b_A[k] *= t;
- }
-
- t = wj;
- }
- }
-
- tau[i] = rankR;
- b_A[i_i] = t;
- if (i + 1 < 3) {
- t = b_A[i_i];
- b_A[i_i] = 1.0;
- i_ip1 = (i + ((i + 1) << 2)) + 1;
- if (tau[i] != 0.0) {
- lastv = 4 - i;
- itemp = i_i - i;
- while ((lastv > 0) && (b_A[itemp + 3] == 0.0)) {
- lastv--;
- itemp--;
- }
-
- lastc = 2 - i;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (lastc > 0)) {
- itemp = i_ip1 + ((lastc - 1) << 2);
- k = itemp;
- do {
- exitg1 = 0;
- if (k <= (itemp + lastv) - 1) {
- if (b_A[k - 1] != 0.0) {
- exitg1 = 1;
- } else {
- k++;
- }
- } else {
- lastc--;
- exitg1 = 2;
- }
- } while (exitg1 == 0);
-
- if (exitg1 == 1) {
- exitg2 = TRUE;
- }
- }
- } else {
- lastv = 0;
- lastc = 0;
- }
-
- if (lastv > 0) {
- if (lastc == 0) {
- } else {
- for (iy = 1; iy <= lastc; iy++) {
- work[iy - 1] = 0.0;
- }
-
- iy = 0;
- i4 = i_ip1 + ((lastc - 1) << 2);
- for (pvt = i_ip1; pvt <= i4; pvt += 4) {
- ix = i_i;
- wj = 0.0;
- itemp = (pvt + lastv) - 1;
- for (k = pvt; k <= itemp; k++) {
- wj += b_A[k - 1] * b_A[ix];
- ix++;
- }
-
- work[iy] += wj;
- iy++;
- }
- }
-
- if (-tau[i] == 0.0) {
- } else {
- itemp = i_ip1 - 1;
- pvt = 0;
- for (iy = 1; iy <= lastc; iy++) {
- if (work[pvt] != 0.0) {
- wj = work[pvt] * -tau[i];
- ix = i_i;
- i4 = lastv + itemp;
- for (k = itemp; k + 1 <= i4; k++) {
- b_A[k] += b_A[ix] * wj;
- ix++;
- }
- }
-
- pvt++;
- itemp += 4;
- }
- }
- }
-
- b_A[i_i] = t;
- }
-
- for (iy = i + 1; iy + 1 < 4; iy++) {
- if (vn1[iy] != 0.0) {
- rankR = fabs(b_A[i + (iy << 2)]) / vn1[iy];
- b_y = rankR * rankR;
- rankR = 1.0 - rankR * rankR;
- if (1.0 - b_y < 0.0) {
- rankR = 0.0;
- }
-
- wj = vn1[iy] / vn2[iy];
- if (rankR * (wj * wj) <= 1.4901161193847656E-8) {
- vn1[iy] = b_eml_xnrm2(3 - i, b_A, (i + (iy << 2)) + 2);
- vn2[iy] = vn1[iy];
- } else {
- vn1[iy] *= sqrt(rankR);
- }
- }
- }
- }
-
- rankR = 0.0;
- k = 0;
- while ((k < 3) && (!(fabs(b_A[k + (k << 2)]) <= 4.0 * fabs(b_A[0]) *
- 2.2204460492503131E-16))) {
- rankR++;
- k++;
- }
-
- memset(&Y[0], 0, 9U * sizeof(real_T));
- for (iy = 0; iy < 3; iy++) {
- if (tau[iy] != 0.0) {
- for (k = 0; k < 3; k++) {
- wj = b_B[iy + (k << 2)];
- for (i = 0; i <= 2 - iy; i++) {
- itemp = (iy + i) + 1;
- wj += b_A[itemp + (iy << 2)] * b_B[itemp + (k << 2)];
- }
-
- wj *= tau[iy];
- if (wj != 0.0) {
- b_B[iy + (k << 2)] -= wj;
- for (i = 0; i <= 2 - iy; i++) {
- itemp = (iy + i) + 1;
- b_B[itemp + (k << 2)] -= b_A[itemp + (iy << 2)] * wj;
- }
- }
- }
- }
- }
-
- for (k = 0; k < 3; k++) {
- for (i = 0; i < (int32_T)rankR; i++) {
- Y[(jpvt[i] + 3 * k) - 1] = b_B[i + (k << 2)];
- }
-
- for (iy = 0; iy < (int32_T)-(1.0 + (-1.0 - rankR)); iy++) {
- wj = rankR + -(real_T)iy;
- Y[(jpvt[(int32_T)wj - 1] + 3 * k) - 1] /= b_A[((int32_T)wj + (((int32_T)wj
- - 1) << 2)) - 1];
- for (i = 0; i <= (int32_T)wj - 2; i++) {
- Y[(jpvt[i] + 3 * k) - 1] -= Y[(jpvt[(int32_T)wj - 1] + 3 * k) - 1] *
- b_A[i + (((int32_T)wj - 1) << 2)];
- }
- }
- }
-
- for (i4 = 0; i4 < 3; i4++) {
- for (itemp = 0; itemp < 3; itemp++) {
- y[itemp + 3 * i4] = Y[i4 + 3 * itemp];
- }
- }
-}
-
-/* End of code generation (mrdivide.cpp) */
diff --git a/.svn/pristine/26/26d14e1a5bddf080c4290eb3d20525c508ba8580.svn-base b/.svn/pristine/26/26d14e1a5bddf080c4290eb3d20525c508ba8580.svn-base
deleted file mode 100644
index 6da7ff9ab696e527bf2b16347e1f114ec2c3f9a9..0000000000000000000000000000000000000000
--- a/.svn/pristine/26/26d14e1a5bddf080c4290eb3d20525c508ba8580.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * round.h
- *
- * Code generation for function 'round'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __ROUND_H__
-#define __ROUND_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_round(real_T x[1296]);
-#endif
-/* End of code generation (round.h) */
diff --git a/.svn/pristine/2c/2c0543c79a50a8ad90a6919d6492c9163a6fd89a.svn-base b/.svn/pristine/2c/2c0543c79a50a8ad90a6919d6492c9163a6fd89a.svn-base
deleted file mode 100644
index 265344e950ad93beab2afe53f596caf14f034c4b..0000000000000000000000000000000000000000
--- a/.svn/pristine/2c/2c0543c79a50a8ad90a6919d6492c9163a6fd89a.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * sqrt.h
- *
- * Code generation for function 'sqrt'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __SQRT_H__
-#define __SQRT_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_sqrt(creal_T *x);
-#endif
-/* End of code generation (sqrt.h) */
diff --git a/.svn/pristine/32/3244c66cdb9d5a50128e4308eea94f4adeecd3cb.svn-base b/.svn/pristine/32/3244c66cdb9d5a50128e4308eea94f4adeecd3cb.svn-base
deleted file mode 100644
index e0ce039472d44c9785f9d4b3e6a162ad32879a2e..0000000000000000000000000000000000000000
--- a/.svn/pristine/32/3244c66cdb9d5a50128e4308eea94f4adeecd3cb.svn-base
+++ /dev/null
@@ -1,235 +0,0 @@
-/*
- * estimateRigidTransform.cpp
- *
- * Code generation for function 'estimateRigidTransform'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "estimateRigidTransform.h"
-#include "quat2rot.h"
-#include "eye.h"
-#include "svd.h"
-#include "crossTimesMatrix.h"
-#include "sum.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void estimateRigidTransform(const real_T x[3888], const real_T y[3888], real_T
- T[16])
-{
- static real_T b_x[3888];
- static real_T b_y[3888];
- int32_T i12;
- int32_T i13;
- real_T x_centroid[3];
- real_T y_centroid[3];
- real_T x_centrized[3888];
- real_T y_centrized[3888];
- real_T b_y_centrized[3888];
- static real_T R22[11664];
- real_T B[16];
- static real_T A[20736];
- int32_T ii;
- real_T d2;
- int32_T i14;
- real_T b_B[16];
- real_T V[16];
- real_T unusedU1[16];
- real_T dv6[9];
- real_T dv7[9];
- real_T dv8[9];
- static const int8_T iv6[4] = { 0, 0, 0, 1 };
-
- real_T dv9[16];
-
- /* ESTIMATERIGIDTRANSFORM */
- /* [T, EPS] = ESTIMATERIGIDTRANSFORM(X, Y) estimates the rigid transformation */
- /* that best aligns x with y (in the least-squares sense). */
- /* */
- /* Reference: "Estimating Rigid Transformations" in */
- /* "Computer Vision, a modern approach" by Forsyth and Ponce (1993), page 480 */
- /* (page 717(?) of the newer edition) */
- /* */
- /* Input: */
- /* X: 3xN, N 3-D points (N>=3) */
- /* Y: 3xN, N 3-D points (N>=3) */
- /* */
- /* Output */
- /* T: the rigid transformation that aligns x and y as: xh = T * yh */
- /* (h denotes homogenous coordinates) */
- /* (corrspondence between points x(:,i) and y(:,i) is assumed) */
- /* */
- /* EPS: the smallest singular value. The closer this value it is */
- /* to 0, the better the estimate is. (large values mean that the */
- /* transform between the two data sets cannot be approximated */
- /* well with a rigid transform. */
- /* */
- /* Babak Taati, 2003 */
- /* (revised 2009) */
- for (i12 = 0; i12 < 1296; i12++) {
- for (i13 = 0; i13 < 3; i13++) {
- b_x[i13 + 3 * i12] = x[i12 + 1296 * i13];
- b_y[i13 + 3 * i12] = y[i12 + 1296 * i13];
- }
- }
-
- /* if nargin ~= 2 */
- /* error('Requires two input arguments.') */
- /* end */
- /* */
- /* if size(x,1)~=3 || size(y,1)~=3 */
- /* error('Input point clouds must be a 3xN matrix.'); */
- /* end */
- /* */
- /* if size(x, 2) ~= size(y,2) */
- /* error('Input point clouds must be of the same size'); */
- /* end */
- /* */
- /* if size(x,2)<3 || size(y,2)<3 */
- /* error('At least 3 point matches are needed'); */
- /* end */
- /* since x has N=3+ points, length shows the number of points */
- sum(b_x, x_centroid);
- sum(b_y, y_centroid);
- for (i12 = 0; i12 < 3; i12++) {
- y_centroid[i12] /= 1296.0;
- x_centroid[i12] /= 1296.0;
- }
-
- for (i12 = 0; i12 < 1296; i12++) {
- x_centrized[3 * i12] = b_x[3 * i12] - x_centroid[0];
- x_centrized[1 + 3 * i12] = b_x[1 + 3 * i12] - x_centroid[1];
- x_centrized[2 + 3 * i12] = b_x[2 + 3 * i12] - x_centroid[2];
- y_centrized[3 * i12] = b_y[3 * i12] - y_centroid[0];
- y_centrized[1 + 3 * i12] = b_y[1 + 3 * i12] - y_centroid[1];
- y_centrized[2 + 3 * i12] = b_y[2 + 3 * i12] - y_centroid[2];
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- for (i13 = 0; i13 < 1296; i13++) {
- b_y[i13 + 1296 * i12] = y_centrized[i12 + 3 * i13] - x_centrized[i12 + 3 *
- i13];
- }
- }
-
- for (i12 = 0; i12 < 3888; i12++) {
- b_x[i12] = x_centrized[i12] - y_centrized[i12];
- b_y_centrized[i12] = y_centrized[i12] + x_centrized[i12];
- }
-
- crossTimesMatrix(b_y_centrized, R22);
- memset(&B[0], 0, sizeof(real_T) << 4);
- memset(&A[0], 0, 20736U * sizeof(real_T));
- for (ii = 0; ii < 1296; ii++) {
- A[ii << 4] = 0.0;
- for (i12 = 0; i12 < 3; i12++) {
- A[((i12 + 1) << 2) + (ii << 4)] = b_y[ii + 1296 * i12];
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- A[(i12 + (ii << 4)) + 1] = b_x[i12 + 3 * ii];
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- for (i13 = 0; i13 < 3; i13++) {
- A[((i13 + ((i12 + 1) << 2)) + (ii << 4)) + 1] = R22[(i13 + 3 * i12) + 9 *
- ii];
- }
- }
-
- for (i12 = 0; i12 < 4; i12++) {
- for (i13 = 0; i13 < 4; i13++) {
- d2 = 0.0;
- for (i14 = 0; i14 < 4; i14++) {
- d2 += A[(i14 + (i12 << 2)) + (ii << 4)] * A[(i14 + (i13 << 2)) + (ii <<
- 4)];
- }
-
- B[i12 + (i13 << 2)] += d2;
- }
- }
- }
-
- memcpy(&b_B[0], &B[0], sizeof(real_T) << 4);
- svd(b_B, B, unusedU1, V);
- b_eye(dv6);
- b_eye(dv7);
- quat2rot(*(real_T (*)[4])&V[12], dv8);
- for (i12 = 0; i12 < 3; i12++) {
- for (i13 = 0; i13 < 3; i13++) {
- unusedU1[i13 + (i12 << 2)] = dv7[i13 + 3 * i12];
- }
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- unusedU1[12 + i12] = x_centroid[i12];
- }
-
- for (i12 = 0; i12 < 4; i12++) {
- unusedU1[3 + (i12 << 2)] = (real_T)iv6[i12];
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- for (i13 = 0; i13 < 3; i13++) {
- V[i13 + (i12 << 2)] = dv8[i13 + 3 * i12];
- }
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- V[12 + i12] = 0.0;
- }
-
- for (i12 = 0; i12 < 4; i12++) {
- V[3 + (i12 << 2)] = (real_T)iv6[i12];
- }
-
- for (i12 = 0; i12 < 4; i12++) {
- for (i13 = 0; i13 < 4; i13++) {
- dv9[i12 + (i13 << 2)] = 0.0;
- for (i14 = 0; i14 < 4; i14++) {
- dv9[i12 + (i13 << 2)] += unusedU1[i12 + (i14 << 2)] * V[i14 + (i13 << 2)];
- }
- }
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- for (i13 = 0; i13 < 3; i13++) {
- unusedU1[i13 + (i12 << 2)] = dv6[i13 + 3 * i12];
- }
- }
-
- for (i12 = 0; i12 < 3; i12++) {
- unusedU1[12 + i12] = -y_centroid[i12];
- }
-
- for (i12 = 0; i12 < 4; i12++) {
- unusedU1[3 + (i12 << 2)] = (real_T)iv6[i12];
- }
-
- for (i12 = 0; i12 < 4; i12++) {
- for (i13 = 0; i13 < 4; i13++) {
- T[i12 + (i13 << 2)] = 0.0;
- for (i14 = 0; i14 < 4; i14++) {
- T[i12 + (i13 << 2)] += dv9[i12 + (i14 << 2)] * unusedU1[i14 + (i13 << 2)];
- }
- }
- }
-
- /* Eps = S(4,4); */
-}
-
-/* End of code generation (estimateRigidTransform.cpp) */
diff --git a/.svn/pristine/35/35cc346aed2cc126725020b9a95f554bb32c7400.svn-base b/.svn/pristine/35/35cc346aed2cc126725020b9a95f554bb32c7400.svn-base
deleted file mode 100644
index f7f37a3b3f15c8a498ee2e837804fc5f7b783938..0000000000000000000000000000000000000000
--- a/.svn/pristine/35/35cc346aed2cc126725020b9a95f554bb32c7400.svn-base
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * rt_nonfinite.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RT_NONFINITE_H__
-#define __RT_NONFINITE_H__
-
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-#include <float.h>
-#endif
-#include <stddef.h>
-#include "rtwtypes.h"
-
-extern real_T rtInf;
-extern real_T rtMinusInf;
-extern real_T rtNaN;
-extern real32_T rtInfF;
-extern real32_T rtMinusInfF;
-extern real32_T rtNaNF;
-extern void rt_InitInfAndNaN(size_t realSize);
-extern boolean_T rtIsInf(real_T value);
-extern boolean_T rtIsInfF(real32_T value);
-extern boolean_T rtIsNaN(real_T value);
-extern boolean_T rtIsNaNF(real32_T value);
-
-typedef struct {
- struct {
- uint32_T wordH;
- uint32_T wordL;
- } words;
-} BigEndianIEEEDouble;
-
-typedef struct {
- struct {
- uint32_T wordL;
- uint32_T wordH;
- } words;
-} LittleEndianIEEEDouble;
-
-typedef struct {
- union {
- real32_T wordLreal;
- uint32_T wordLuint;
- } wordL;
-} IEEESingle;
-
-#endif
-/* End of code generation (rt_nonfinite.h) */
diff --git a/.svn/pristine/43/43b7b714ed92cd2b3010bbf7055ac637bcafbe67.svn-base b/.svn/pristine/43/43b7b714ed92cd2b3010bbf7055ac637bcafbe67.svn-base
deleted file mode 100644
index 19affb4cdd724c7bf1c4fbbeed72a14b32277333..0000000000000000000000000000000000000000
--- a/.svn/pristine/43/43b7b714ed92cd2b3010bbf7055ac637bcafbe67.svn-base
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * crossTimesMatrix.cpp
- *
- * Code generation for function 'crossTimesMatrix'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "crossTimesMatrix.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void crossTimesMatrix(const real_T V[3888], real_T V_times[11664])
-{
- int32_T i15;
-
- /* CROSSTIMESMATRIX */
- /* V_TIMES = CROSSTIMESMATRIX(V) returns a 3x3 (or a series of 3x3) cross times matrices of input vector(s) V */
- /* */
- /* Input: */
- /* V a 3xN matrix, rpresenting a series of 3x1 vectors */
- /* */
- /* Output: */
- /* V_TIMES (Vx) a series of 3x3 matrices where V_times(:,:,i) is the Vx matrix for the vector V(:,i) */
- /* */
- /* Babak Taati, 2003 */
- /* (revised 2009) */
- memset(&V_times[0], 0, 11664U * sizeof(real_T));
-
- /* V_times(1,1,:) = 0; */
- for (i15 = 0; i15 < 1296; i15++) {
- V_times[3 + 9 * i15] = -V[2 + 3 * i15];
- V_times[6 + 9 * i15] = V[1 + 3 * i15];
- V_times[1 + 9 * i15] = V[2 + 3 * i15];
-
- /* V_times(2,2,:) = 0; */
- V_times[7 + 9 * i15] = -V[3 * i15];
- V_times[2 + 9 * i15] = -V[1 + 3 * i15];
- V_times[5 + 9 * i15] = V[3 * i15];
- }
-
- /* V_times(3,3,:) = 0; */
-}
-
-/* End of code generation (crossTimesMatrix.cpp) */
diff --git a/.svn/pristine/49/4952aa6db3d613cdb82cfb293b24a1ecb736bc41.svn-base b/.svn/pristine/49/4952aa6db3d613cdb82cfb293b24a1ecb736bc41.svn-base
deleted file mode 100644
index bd8d2e386bce61b73a5f91003dec9654a614d06b..0000000000000000000000000000000000000000
--- a/.svn/pristine/49/4952aa6db3d613cdb82cfb293b24a1ecb736bc41.svn-base
+++ /dev/null
@@ -1,104 +0,0 @@
-/*
- * mldivide.cpp
- *
- * Code generation for function 'mldivide'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "mldivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_mldivide(const real_T A[4], const real_T B[4], real_T Y[4])
-{
- int32_T r1;
- int32_T r2;
- real_T a21;
- real_T a22;
- int32_T k;
- if (fabs(A[1]) > fabs(A[0])) {
- r1 = 1;
- r2 = 0;
- } else {
- r1 = 0;
- r2 = 1;
- }
-
- a21 = A[r2] / A[r1];
- a22 = A[2 + r2] - a21 * A[2 + r1];
- for (k = 0; k < 2; k++) {
- Y[1 + (k << 1)] = (B[r2 + (k << 1)] - B[r1 + (k << 1)] * a21) / a22;
- Y[k << 1] = (B[r1 + (k << 1)] - Y[1 + (k << 1)] * A[2 + r1]) / A[r1];
- }
-}
-
-void mldivide(const real_T A[9], const real_T B[9], real_T Y[9])
-{
- real_T b_A[9];
- int32_T r1;
- int32_T r2;
- int32_T r3;
- real_T maxval;
- real_T a21;
- int32_T rtemp;
- memcpy(&b_A[0], (real_T *)&A[0], 9U * sizeof(real_T));
- r1 = 0;
- r2 = 1;
- r3 = 2;
- maxval = fabs(A[0]);
- a21 = fabs(A[1]);
- if (a21 > maxval) {
- maxval = a21;
- r1 = 1;
- r2 = 0;
- }
-
- if (fabs(A[2]) > maxval) {
- r1 = 2;
- r2 = 1;
- r3 = 0;
- }
-
- b_A[r2] = A[r2] / A[r1];
- b_A[r3] /= b_A[r1];
- b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
- b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
- b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
- b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
- if (fabs(b_A[3 + r3]) > fabs(b_A[3 + r2])) {
- rtemp = r2;
- r2 = r3;
- r3 = rtemp;
- }
-
- b_A[3 + r3] /= b_A[3 + r2];
- b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
- for (rtemp = 0; rtemp < 3; rtemp++) {
- Y[3 * rtemp] = B[r1 + 3 * rtemp];
- Y[1 + 3 * rtemp] = B[r2 + 3 * rtemp] - Y[3 * rtemp] * b_A[r2];
- Y[2 + 3 * rtemp] = (B[r3 + 3 * rtemp] - Y[3 * rtemp] * b_A[r3]) - Y[1 + 3 *
- rtemp] * b_A[3 + r3];
- Y[2 + 3 * rtemp] /= b_A[6 + r3];
- Y[3 * rtemp] -= Y[2 + 3 * rtemp] * b_A[6 + r1];
- Y[1 + 3 * rtemp] -= Y[2 + 3 * rtemp] * b_A[6 + r2];
- Y[1 + 3 * rtemp] /= b_A[3 + r2];
- Y[3 * rtemp] -= Y[1 + 3 * rtemp] * b_A[3 + r1];
- Y[3 * rtemp] /= b_A[r1];
- }
-}
-
-/* End of code generation (mldivide.cpp) */
diff --git a/.svn/pristine/49/496c94545f5d6ac147083320a46496bc2e19ce28.svn-base b/.svn/pristine/49/496c94545f5d6ac147083320a46496bc2e19ce28.svn-base
deleted file mode 100644
index dc92eac00e5ef1d041b6eb4c36504e991200c1a7..0000000000000000000000000000000000000000
--- a/.svn/pristine/49/496c94545f5d6ac147083320a46496bc2e19ce28.svn-base
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_rtwutil.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_rtwutil'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-real_T rt_hypotd_snf(real_T u0, real_T u1)
-{
- real_T y;
- real_T a;
- real_T b;
- a = fabs(u0);
- b = fabs(u1);
- if (a < b) {
- a /= b;
- y = b * sqrt(a * a + 1.0);
- } else if (a > b) {
- b /= a;
- y = a * sqrt(b * b + 1.0);
- } else if (rtIsNaN(b)) {
- y = b;
- } else {
- y = a * 1.4142135623730951;
- }
-
- return y;
-}
-
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_rtwutil.cpp) */
diff --git a/.svn/pristine/4b/4be140c1a5babc0318feadc46ae0b70e4f912047.svn-base b/.svn/pristine/4b/4be140c1a5babc0318feadc46ae0b70e4f912047.svn-base
deleted file mode 100644
index a1aad2ca125d6b2742d6e91e29f358fea39266ed..0000000000000000000000000000000000000000
--- a/.svn/pristine/4b/4be140c1a5babc0318feadc46ae0b70e4f912047.svn-base
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * cumulative_sum.cpp
- *
- * Code generation for function 'cumulative_sum'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "cumulative_sum.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void cumulative_sum(const real_T x[25], real_T csum[25])
-{
- int32_T i;
- memset(&csum[0], 0, 25U * sizeof(real_T));
- csum[0] = x[0];
- for (i = 0; i < 24; i++) {
- csum[1 + i] = x[1 + i] + csum[i];
- }
-}
-
-/* End of code generation (cumulative_sum.cpp) */
diff --git a/.svn/pristine/4b/4bf9f7f24e74fabf362ebf1857b995815748b6d8.svn-base b/.svn/pristine/4b/4bf9f7f24e74fabf362ebf1857b995815748b6d8.svn-base
deleted file mode 100644
index 5d486ddabb44939cb54e31dd12a1be0c86f3dc5d..0000000000000000000000000000000000000000
--- a/.svn/pristine/4b/4bf9f7f24e74fabf362ebf1857b995815748b6d8.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * norm.h
- *
- * Code generation for function 'norm'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __NORM_H__
-#define __NORM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real_T b_norm(const real_T x[3888]);
-extern real_T norm(const real_T x[1296]);
-#endif
-/* End of code generation (norm.h) */
diff --git a/.svn/pristine/4e/4e16d9f167262ea065a4fb9eb104a9bd24216faf.svn-base b/.svn/pristine/4e/4e16d9f167262ea065a4fb9eb104a9bd24216faf.svn-base
deleted file mode 100644
index 28ef07d8361059baeb9e00df039862b3aa0812cb..0000000000000000000000000000000000000000
--- a/.svn/pristine/4e/4e16d9f167262ea065a4fb9eb104a9bd24216faf.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * crossTimesMatrix.h
- *
- * Code generation for function 'crossTimesMatrix'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __CROSSTIMESMATRIX_H__
-#define __CROSSTIMESMATRIX_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void crossTimesMatrix(const real_T V[3888], real_T V_times[11664]);
-#endif
-/* End of code generation (crossTimesMatrix.h) */
diff --git a/.svn/pristine/57/57912c4599258cb2c5803f94cf2ad182747794ca.svn-base b/.svn/pristine/57/57912c4599258cb2c5803f94cf2ad182747794ca.svn-base
deleted file mode 100644
index 2b6b14e74d9f22e139b38b974156f170b4c19cee..0000000000000000000000000000000000000000
--- a/.svn/pristine/57/57912c4599258cb2c5803f94cf2ad182747794ca.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * svd.h
- *
- * Code generation for function 'svd'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __SVD_H__
-#define __SVD_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void svd(const real_T A[16], real_T U[16], real_T S[16], real_T V[16]);
-#endif
-/* End of code generation (svd.h) */
diff --git a/.svn/pristine/5b/5b6c39a2ba523a6aa9487363a9958bc6dc929894.svn-base b/.svn/pristine/5b/5b6c39a2ba523a6aa9487363a9958bc6dc929894.svn-base
deleted file mode 100644
index b0a740da78e8430673d9d729d99a4fd10626d90c..0000000000000000000000000000000000000000
--- a/.svn/pristine/5b/5b6c39a2ba523a6aa9487363a9958bc6dc929894.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * mldivide.h
- *
- * Code generation for function 'mldivide'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __MLDIVIDE_H__
-#define __MLDIVIDE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_mldivide(const real_T A[4], const real_T B[4], real_T Y[4]);
-extern void mldivide(const real_T A[9], const real_T B[9], real_T Y[9]);
-#endif
-/* End of code generation (mldivide.h) */
diff --git a/.svn/pristine/6c/6c84567f93e9c383d37697442e6026ae4c90abd8.svn-base b/.svn/pristine/6c/6c84567f93e9c383d37697442e6026ae4c90abd8.svn-base
deleted file mode 100644
index eaf6d1fa70de12019f06561d8a024be165486853..0000000000000000000000000000000000000000
--- a/.svn/pristine/6c/6c84567f93e9c383d37697442e6026ae4c90abd8.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * compute_prob1.h
- *
- * Code generation for function 'compute_prob1'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __COMPUTE_PROB1_H__
-#define __COMPUTE_PROB1_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void compute_prob1(real_T X_par[2250000], real_T X_par_pred[2250000], real_T AR_velocity[2250000], const real_T dw_dp[15552], real_T t, real_T center_x, real_T center_y, const real_T Ixyz[504063], const real_T point_matrix[3888], const real_T mean_img[3888], real_T tracked_images[38880000], real_T Aff_matrix[9], real_T centroid[3]);
-extern void eml_li_find(const boolean_T x[1296], int32_T y_data[1296], int32_T y_size[1]);
-#endif
-/* End of code generation (compute_prob1.h) */
diff --git a/.svn/pristine/6f/6f02aafdeb1e6ce10d299378a03f63bcfe70c7b5.svn-base b/.svn/pristine/6f/6f02aafdeb1e6ce10d299378a03f63bcfe70c7b5.svn-base
deleted file mode 100644
index a87d1b44bc1f5f0c41dd4f66ccbd4427b007ac1b..0000000000000000000000000000000000000000
--- a/.svn/pristine/6f/6f02aafdeb1e6ce10d299378a03f63bcfe70c7b5.svn-base
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * meshgrid.cpp
- *
- * Code generation for function 'meshgrid'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "meshgrid.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void meshgrid(const real_T x[25], const real_T y[25], real_T xx[625], real_T yy
- [625])
-{
- int32_T ia;
- int32_T ib;
- int32_T iacol;
- int32_T jcol;
- int32_T itilerow;
- ia = 1;
- ib = 0;
- iacol = 1;
- for (jcol = 0; jcol < 25; jcol++) {
- for (itilerow = 0; itilerow < 25; itilerow++) {
- xx[ib] = x[iacol - 1];
- ia = iacol + 1;
- ib++;
- }
-
- iacol = ia;
- }
-
- ib = 0;
- for (iacol = 0; iacol < 25; iacol++) {
- ia = 0;
- for (jcol = 0; jcol < 25; jcol++) {
- yy[ib] = y[ia];
- ia++;
- ib++;
- }
- }
-}
-
-/* End of code generation (meshgrid.cpp) */
diff --git a/.svn/pristine/6f/6f3ada8a5ec96ecc7f6de67c37fe5e4a5c86debe.svn-base b/.svn/pristine/6f/6f3ada8a5ec96ecc7f6de67c37fe5e4a5c86debe.svn-base
deleted file mode 100644
index b358c87fe7e622f20ecdb5fb10069e457abf7689..0000000000000000000000000000000000000000
--- a/.svn/pristine/6f/6f3ada8a5ec96ecc7f6de67c37fe5e4a5c86debe.svn-base
+++ /dev/null
@@ -1,47 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-set(ROS_BUILD_TYPE Release)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pipe -std=c++0x -fopenmp -fPIC")
-
-rosbuild_add_library(lmath src/chol.cpp src/compute_prob1.cpp
- src/crossTimesMatrix.cpp src/cumulative_sum.cpp src/diag.cpp src/eig.cpp
- src/estimateRigidTransform.cpp src/exp.cpp src/expm.cpp src/eye.cpp src/gradient.cpp
- src/image_warping.cpp src/init_variables.cpp src/log.cpp src/mean.cpp src/meshgrid.cpp src/mldivide.cpp src/mrdivide.cpp
- src/norm.cpp src/Optimal_affine_tracking_3d16_fast_realtime_data.cpp
- src/Optimal_affine_tracking_3d16_fast_realtime_initialize.cpp
- src/Optimal_affine_tracking_3d16_fast_realtime_rtwutil.cpp
- src/Optimal_affine_tracking_3d16_fast_realtime_terminate.cpp src/quat2rot.cpp
- src/rand.cpp src/randn.cpp src/repmat.cpp src/resampling.cpp src/round.cpp src/rtGetNaN.cpp
- src/rtGetInf.cpp src/rt_nonfinite.cpp src/sqrt.cpp src/sum.cpp src/svd.cpp)
-
-
-
-rosbuild_add_executable(depth_tracker src/main.cpp)
-target_link_libraries(depth_tracker lmath)
-rosbuild_add_openmp_flags(depth_tracker)
diff --git a/.svn/pristine/71/71bcccd2efe711e112f0e4b8e1c2465a86133a6d.svn-base b/.svn/pristine/71/71bcccd2efe711e112f0e4b8e1c2465a86133a6d.svn-base
deleted file mode 100644
index b75b928f20ef9ea4f509a17db62e4e31b422c27f..0000000000000000000000000000000000000000
--- a/.svn/pristine/71/71bcccd2efe711e112f0e4b8e1c2465a86133a6d.svn-base
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/.svn/pristine/73/73b2f912170aff1b742fe125993a7acb88bdd548.svn-base b/.svn/pristine/73/73b2f912170aff1b742fe125993a7acb88bdd548.svn-base
deleted file mode 100644
index 23024e67fb088b350a92239a6e5f9710f0f3b3b0..0000000000000000000000000000000000000000
--- a/.svn/pristine/73/73b2f912170aff1b742fe125993a7acb88bdd548.svn-base
+++ /dev/null
@@ -1,135 +0,0 @@
-/*
- * image_warping.cpp
- *
- * Code generation for function 'image_warping'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "image_warping.h"
-#include "round.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_image_warping(const real_T frame[336042], const real_T aff_matrix[9],
- const real_T point_matrix[3888], real_T warped_img[1296])
-{
- int32_T k;
- int32_T i10;
- real_T transformed_point[3888];
- int32_T i11;
- real_T maxval[1296];
- real_T b_maxval[1296];
- real_T u0;
- for (k = 0; k < 3; k++) {
- for (i10 = 0; i10 < 1296; i10++) {
- transformed_point[k + 3 * i10] = 0.0;
- for (i11 = 0; i11 < 3; i11++) {
- transformed_point[k + 3 * i10] += aff_matrix[k + 3 * i11] *
- point_matrix[i11 + 3 * i10];
- }
- }
- }
-
- for (k = 0; k < 1296; k++) {
- u0 = transformed_point[3 * k];
- if (u0 <= 381.0) {
- } else {
- u0 = 381.0;
- }
-
- if ((1.0 >= u0) || rtIsNaN(u0)) {
- u0 = 1.0;
- }
-
- maxval[k] = u0;
- u0 = transformed_point[1 + 3 * k];
- if (u0 <= 882.0) {
- } else {
- u0 = 882.0;
- }
-
- if ((1.0 >= u0) || rtIsNaN(u0)) {
- u0 = 1.0;
- }
-
- b_maxval[k] = u0;
- }
-
- b_round(maxval);
- b_round(b_maxval);
- for (k = 0; k < 1296; k++) {
- warped_img[k] = frame[(int32_T)((b_maxval[k] - 1.0) * 381.0 + maxval[k]) - 1];
- }
-
- /* warped_img = reshape(frame((y-1)*size_x + x),[crop_x crop_y]); */
-}
-
-void image_warping(const real_T frame[168021], const real_T aff_matrix[9], const
- real_T point_matrix[3888], real_T warped_img[1296])
-{
- int32_T k;
- int32_T i5;
- real_T transformed_point[3888];
- int32_T i6;
- real_T maxval[1296];
- real_T b_maxval[1296];
- real_T u0;
- for (k = 0; k < 3; k++) {
- for (i5 = 0; i5 < 1296; i5++) {
- transformed_point[k + 3 * i5] = 0.0;
- for (i6 = 0; i6 < 3; i6++) {
- transformed_point[k + 3 * i5] += aff_matrix[k + 3 * i6] *
- point_matrix[i6 + 3 * i5];
- }
- }
- }
-
- for (k = 0; k < 1296; k++) {
- u0 = transformed_point[3 * k];
- if (u0 <= 381.0) {
- } else {
- u0 = 381.0;
- }
-
- if ((1.0 >= u0) || rtIsNaN(u0)) {
- u0 = 1.0;
- }
-
- maxval[k] = u0;
- u0 = transformed_point[1 + 3 * k];
- if (u0 <= 441.0) {
- } else {
- u0 = 441.0;
- }
-
- if ((1.0 >= u0) || rtIsNaN(u0)) {
- u0 = 1.0;
- }
-
- b_maxval[k] = u0;
- }
-
- b_round(maxval);
- b_round(b_maxval);
- for (k = 0; k < 1296; k++) {
- warped_img[k] = frame[(int32_T)((b_maxval[k] - 1.0) * 381.0 + maxval[k]) - 1];
- }
-
- /* warped_img = reshape(frame((y-1)*size_x + x),[crop_x crop_y]); */
-}
-
-/* End of code generation (image_warping.cpp) */
diff --git a/.svn/pristine/75/758d56be60baa8958fddd99be2b873ee423113eb.svn-base b/.svn/pristine/75/758d56be60baa8958fddd99be2b873ee423113eb.svn-base
deleted file mode 100644
index 519a4027bb6a4bd65f986d7336f4792bfdd52a2f..0000000000000000000000000000000000000000
--- a/.svn/pristine/75/758d56be60baa8958fddd99be2b873ee423113eb.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * meshgrid.h
- *
- * Code generation for function 'meshgrid'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __MESHGRID_H__
-#define __MESHGRID_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void meshgrid(const real_T x[25], const real_T y[25], real_T xx[625], real_T yy[625]);
-#endif
-/* End of code generation (meshgrid.h) */
diff --git a/.svn/pristine/77/772b74fef53df1b54e63773a1db8ad4d8faf5ade.svn-base b/.svn/pristine/77/772b74fef53df1b54e63773a1db8ad4d8faf5ade.svn-base
deleted file mode 100644
index 6f9b05c41ec5a6fedacb6fe32bf4d5199863d63b..0000000000000000000000000000000000000000
--- a/.svn/pristine/77/772b74fef53df1b54e63773a1db8ad4d8faf5ade.svn-base
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * gradient.cpp
- *
- * Code generation for function 'gradient'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "gradient.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void gradient(const real_T f[168021], real_T varargout_1[168021], real_T
- varargout_2[168021])
-{
- int32_T i1;
- int32_T i2;
- int32_T j;
- int32_T k;
- i1 = -1;
- i2 = 167639;
- for (j = 0; j < 381; j++) {
- i1++;
- i2++;
- varargout_1[i1] = f[i1 + 381] - f[i1];
- for (k = 0; k < 439; k++) {
- varargout_1[i1 + (k + 1) * 381] = (f[i1 + (k + 2) * 381] - f[i1 + k * 381])
- / 2.0;
- }
-
- varargout_1[i2] = f[i2] - f[i2 - 381];
- }
-
- i2 = -1;
- for (j = 0; j < 441; j++) {
- i1 = i2 + 1;
- i2 += 381;
- varargout_2[i1] = f[i1 + 1] - f[i1];
- for (k = 0; k < 379; k++) {
- varargout_2[(i1 + k) + 1] = (f[(i1 + k) + 2] - f[i1 + k]) / 2.0;
- }
-
- varargout_2[i2] = f[i2] - f[i2 - 1];
- }
-}
-
-/* End of code generation (gradient.cpp) */
diff --git a/.svn/pristine/78/789dba8225130ece99b05777ea8c8d17a8fa224a.svn-base b/.svn/pristine/78/789dba8225130ece99b05777ea8c8d17a8fa224a.svn-base
deleted file mode 100644
index 3910db4c823089878f676a481b9400668064fc89..0000000000000000000000000000000000000000
--- a/.svn/pristine/78/789dba8225130ece99b05777ea8c8d17a8fa224a.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * chol.h
- *
- * Code generation for function 'chol'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __CHOL_H__
-#define __CHOL_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void chol(real_T A[36]);
-#endif
-/* End of code generation (chol.h) */
diff --git a/.svn/pristine/7a/7accec2c7862a5b620c70933bb20ced875ca31fa.svn-base b/.svn/pristine/7a/7accec2c7862a5b620c70933bb20ced875ca31fa.svn-base
deleted file mode 100644
index 9a513282b748857b012a9f152fdacc1da9997abd..0000000000000000000000000000000000000000
--- a/.svn/pristine/7a/7accec2c7862a5b620c70933bb20ced875ca31fa.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * log.h
- *
- * Code generation for function 'log'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __LOG_H__
-#define __LOG_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_log(creal_T x[3]);
-extern void c_log(creal_T x[2]);
-#endif
-/* End of code generation (log.h) */
diff --git a/.svn/pristine/7e/7e072cef2d93754ec15793a98eb79e62f8810e4c.svn-base b/.svn/pristine/7e/7e072cef2d93754ec15793a98eb79e62f8810e4c.svn-base
deleted file mode 100644
index c7539212d017b7d89e1d093c7029d730dd058f69..0000000000000000000000000000000000000000
--- a/.svn/pristine/7e/7e072cef2d93754ec15793a98eb79e62f8810e4c.svn-base
+++ /dev/null
@@ -1,685 +0,0 @@
-/*
- * init_variables.cpp
- *
- * Code generation for function 'init_variables'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "init_variables.h"
-#include "eye.h"
-#include "mean.h"
-#include "compute_prob1.h"
-#include "image_warping.h"
-#include "mrdivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void init_variables(const real_T Ixyz[504063], const real_T ptx[4], const real_T
- pty[4], real_T X_par_pred[2250000], real_T tracked_images
- [38880000], real_T dw_dp[15552], real_T X_par[2250000],
- real_T AR_velocity[2250000], real_T point_matrix[3888],
- real_T mean_img[3888], real_T corner_p[12], real_T *center_x,
- real_T *center_y)
-{
- static real_T frame[504063];
- static real_T b_Ixyz[168021];
- static real_T c_Ixyz[168021];
- int32_T row;
- int32_T col;
- int32_T i;
- static const real_T dv0[1296] = { -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5 };
-
- static const real_T dv1[1296] = { -17.5, -17.5, -17.5, -17.5, -17.5, -17.5,
- -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5,
- -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5,
- -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -16.5, -16.5, -16.5,
- -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5,
- -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5,
- -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5,
- -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5,
- -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5,
- -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5,
- -15.5, -15.5, -15.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5,
- -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5,
- -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5,
- -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -13.5, -13.5, -13.5, -13.5, -13.5,
- -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5,
- -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5,
- -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -12.5, -12.5,
- -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5,
- -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5,
- -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5,
- -12.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5,
- -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5,
- -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5,
- -11.5, -11.5, -11.5, -11.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5,
- -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5,
- -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5,
- -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -9.5, -9.5, -9.5, -9.5,
- -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5,
- -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5,
- -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5,
- -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5,
- -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5,
- -8.5, -8.5, -8.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5,
- -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5,
- -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5,
- -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5,
- -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5,
- -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -5.5, -5.5, -5.5,
- -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5,
- -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5,
- -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5,
- -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5,
- -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5,
- -4.5, -4.5, -4.5, -4.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5,
- -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5,
- -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5,
- -3.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5,
- -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5,
- -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -1.5, -1.5,
- -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5,
- -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5,
- -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -0.5, -0.5, -0.5, -0.5, -0.5,
- -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
- -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
- -0.5, -0.5, -0.5, -0.5, -0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
- 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
- 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 1.5, 1.5, 1.5,
- 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5,
- 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5,
- 1.5, 1.5, 1.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5,
- 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5,
- 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5,
- 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5,
- 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5,
- 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5,
- 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5,
- 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5,
- 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5,
- 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 6.5, 6.5, 6.5,
- 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5,
- 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5,
- 6.5, 6.5, 6.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5,
- 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5,
- 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5,
- 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5,
- 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5,
- 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5,
- 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5,
- 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5,
- 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5,
- 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5,
- 10.5, 10.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5,
- 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5,
- 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 12.5,
- 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5,
- 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5,
- 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 13.5, 13.5, 13.5, 13.5,
- 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5,
- 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5,
- 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5,
- 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5,
- 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5,
- 14.5, 14.5, 14.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5,
- 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5,
- 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5,
- 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5,
- 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5,
- 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 17.5, 17.5, 17.5,
- 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5,
- 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5,
- 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5 };
-
- real_T origin_p[12];
- static const real_T dv2[3] = { -17.5, -17.5, 1.0 };
-
- static const real_T dv3[3] = { -17.5, 17.5, 1.0 };
-
- static const real_T dv4[3] = { 17.5, 17.5, 1.0 };
-
- static const real_T dv5[3] = { 17.5, -17.5, 1.0 };
-
- real_T init_ratio[9];
- real_T b_init_ratio[3888];
- real_T b_center_x[2];
- static const int8_T iv0[4] = { 1, 0, 0, 1 };
-
- static const int8_T iv1[3] = { 0, 0, 1 };
-
- boolean_T zero_ind_z[1296];
- boolean_T b_zero_ind_z;
- boolean_T c_zero_ind_z[1296];
- int32_T tmp_size[1];
- int32_T tmp_data[1296];
- int32_T b_tmp_size[1];
- int32_T b_tmp_data[1296];
- real_T mean_img_data[1296];
- int32_T mean_img_size[1];
- real_T d1;
- int32_T b_mean_img_size[1];
- int32_T c_mean_img_size[1];
- static const real_T mx[1296] = { -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5,
- -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5,
- 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5,
- 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5,
- -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5,
- 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5,
- 16.5, 17.5, -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5,
- -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5,
- 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5,
- -17.5, -16.5, -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5,
- -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5,
- 7.5, 8.5, 9.5, 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5,
- -15.5, -14.5, -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5,
- -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5,
- 10.5, 11.5, 12.5, 13.5, 14.5, 15.5, 16.5, 17.5, -17.5, -16.5, -15.5, -14.5,
- -13.5, -12.5, -11.5, -10.5, -9.5, -8.5, -7.5, -6.5, -5.5, -4.5, -3.5, -2.5,
- -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5, 9.5, 10.5, 11.5,
- 12.5, 13.5, 14.5, 15.5, 16.5, 17.5 };
-
- static const real_T my[1296] = { -17.5, -17.5, -17.5, -17.5, -17.5, -17.5,
- -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5,
- -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5,
- -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -17.5, -16.5, -16.5, -16.5,
- -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5,
- -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5,
- -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5, -16.5,
- -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5,
- -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5,
- -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5, -15.5,
- -15.5, -15.5, -15.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5,
- -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5,
- -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -14.5,
- -14.5, -14.5, -14.5, -14.5, -14.5, -14.5, -13.5, -13.5, -13.5, -13.5, -13.5,
- -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5,
- -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5,
- -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -13.5, -12.5, -12.5,
- -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5,
- -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5,
- -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5, -12.5,
- -12.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5,
- -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5,
- -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5, -11.5,
- -11.5, -11.5, -11.5, -11.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5,
- -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5,
- -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5,
- -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -10.5, -9.5, -9.5, -9.5, -9.5,
- -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5,
- -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -9.5,
- -9.5, -9.5, -9.5, -9.5, -9.5, -9.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5,
- -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5,
- -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5, -8.5,
- -8.5, -8.5, -8.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5,
- -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5,
- -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5, -7.5,
- -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5,
- -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5,
- -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -6.5, -5.5, -5.5, -5.5,
- -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5,
- -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5,
- -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -5.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5,
- -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5,
- -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5, -4.5,
- -4.5, -4.5, -4.5, -4.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5,
- -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5,
- -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5, -3.5,
- -3.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5,
- -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5,
- -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5, -1.5, -1.5,
- -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5,
- -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5,
- -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5, -0.5, -0.5, -0.5, -0.5, -0.5,
- -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
- -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
- -0.5, -0.5, -0.5, -0.5, -0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
- 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
- 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 1.5, 1.5, 1.5,
- 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5,
- 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5,
- 1.5, 1.5, 1.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5,
- 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5,
- 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5,
- 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5,
- 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5,
- 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5,
- 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5,
- 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5,
- 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5,
- 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 5.5, 6.5, 6.5, 6.5,
- 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5,
- 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5, 6.5,
- 6.5, 6.5, 6.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5,
- 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5,
- 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 7.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5,
- 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5,
- 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5, 8.5,
- 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5,
- 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 9.5,
- 9.5, 9.5, 9.5, 9.5, 9.5, 9.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5,
- 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5,
- 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5, 10.5,
- 10.5, 10.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5,
- 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5,
- 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 12.5,
- 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5,
- 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5,
- 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 12.5, 13.5, 13.5, 13.5, 13.5,
- 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5,
- 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 13.5,
- 13.5, 13.5, 13.5, 13.5, 13.5, 13.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5,
- 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5,
- 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5, 14.5,
- 14.5, 14.5, 14.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5,
- 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5,
- 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5, 15.5,
- 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5,
- 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5,
- 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 16.5, 17.5, 17.5, 17.5,
- 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5,
- 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5,
- 17.5, 17.5, 17.5, 17.5, 17.5, 17.5, 17.5 };
-
- /* % */
- *center_x = (((ptx[0] + ptx[1]) + ptx[2]) + ptx[3]) / 4.0;
- *center_y = (((pty[0] + pty[1]) + pty[2]) + pty[3]) / 4.0;
- memset(&frame[0], 0, 504063U * sizeof(real_T));
- for (row = 0; row < 441; row++) {
- for (col = 0; col < 381; col++) {
- frame[336042 + (col + 381 * row)] = Ixyz[336042 + (col + 381 * row)];
- b_Ixyz[col + 381 * row] = (Ixyz[col + 381 * row] + 2.0) * (real_T)(frame
- [336042 + (col + 381 * row)] != 0.0);
- frame[col + 381 * row] = b_Ixyz[col + 381 * row];
- c_Ixyz[col + 381 * row] = (Ixyz[168021 + (col + 381 * row)] + 2.0) *
- (real_T)(frame[336042 + (col + 381 * row)] != 0.0);
- frame[168021 + (col + 381 * row)] = c_Ixyz[col + 381 * row];
- }
- }
-
- /* % Computing initial 2-D affine transformation matrix */
- memset(&point_matrix[0], 0, 3888U * sizeof(real_T));
- for (i = 0; i < 1296; i++) {
- point_matrix[3 * i] = dv0[i];
- point_matrix[1 + 3 * i] = dv1[i];
- point_matrix[2 + 3 * i] = 1.0;
- }
-
- memset(&origin_p[0], 0, 12U * sizeof(real_T));
- for (row = 0; row < 3; row++) {
- origin_p[row] = dv2[row];
- origin_p[3 + row] = dv3[row];
- origin_p[6 + row] = dv4[row];
- origin_p[9 + row] = dv5[row];
- }
-
- corner_p[0] = ptx[0] - *center_x;
- corner_p[1] = pty[0] - *center_y;
- corner_p[2] = 1.0;
- corner_p[3] = ptx[1] - *center_x;
- corner_p[4] = pty[1] - *center_y;
- corner_p[5] = 1.0;
- corner_p[6] = ptx[2] - *center_x;
- corner_p[7] = pty[2] - *center_y;
- corner_p[8] = 1.0;
- corner_p[9] = ptx[3] - *center_x;
- corner_p[10] = pty[3] - *center_y;
- corner_p[11] = 1.0;
- mrdivide(corner_p, origin_p, init_ratio);
- for (row = 0; row < 3; row++) {
- for (col = 0; col < 1296; col++) {
- b_init_ratio[row + 3 * col] = 0.0;
- for (i = 0; i < 3; i++) {
- b_init_ratio[row + 3 * col] += init_ratio[row + 3 * i] * point_matrix[i
- + 3 * col];
- }
- }
- }
-
- for (row = 0; row < 1296; row++) {
- for (col = 0; col < 3; col++) {
- point_matrix[col + 3 * row] = b_init_ratio[col + 3 * row];
- }
- }
-
- /* % Lie algebra bases for 2-D affine group %% */
- /* scaling */
- /* aspect ratio */
- /* rotation */
- /* skew */
- /* x translation */
- /* y translation */
- /* % Setup for geometric particle filtering %% */
- for (row = 0; row < 2250000; row++) {
- X_par[row] = 0.0;
- X_par_pred[row] = 0.0;
- AR_velocity[row] = 0.0;
- }
-
- /* w = zeros(N,len); */
- /* prob = zeros(N,len); */
- /* % Setup for incremental PCA of image covariances */
- b_center_x[0] = *center_x;
- b_center_x[1] = *center_y;
- for (row = 0; row < 2; row++) {
- for (col = 0; col < 2; col++) {
- init_ratio[col + 3 * row] = (real_T)iv0[col + (row << 1)];
- }
- }
-
- for (row = 0; row < 2; row++) {
- init_ratio[6 + row] = b_center_x[row];
- }
-
- for (row = 0; row < 3; row++) {
- init_ratio[2 + 3 * row] = (real_T)iv1[row];
- }
-
- memset(&mean_img[0], 0, 3888U * sizeof(real_T));
- image_warping(*(real_T (*)[168021])&frame[0], init_ratio, point_matrix,
- *(real_T (*)[1296])&mean_img[0]);
- image_warping(*(real_T (*)[168021])&frame[168021], init_ratio, point_matrix,
- *(real_T (*)[1296])&mean_img[1296]);
- image_warping(*(real_T (*)[168021])&frame[336042], init_ratio, point_matrix,
- *(real_T (*)[1296])&mean_img[2592]);
-
- /* % replace zeros with average values */
- for (row = 0; row < 1296; row++) {
- b_zero_ind_z = (mean_img[2592 + row] == 0.0);
- zero_ind_z[row] = !b_zero_ind_z;
- c_zero_ind_z[row] = b_zero_ind_z;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- mean_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (row = 0; row < i; row++) {
- mean_img_data[row] = mean_img[b_tmp_data[row] + 2591];
- }
-
- d1 = mean(mean_img_data, mean_img_size);
- i = tmp_size[0];
- for (row = 0; row < i; row++) {
- mean_img[tmp_data[row] + 2591] = d1;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- b_mean_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (row = 0; row < i; row++) {
- mean_img_data[row] = mean_img[b_tmp_data[row] - 1];
- }
-
- d1 = mean(mean_img_data, b_mean_img_size);
- i = tmp_size[0];
- for (row = 0; row < i; row++) {
- mean_img[tmp_data[row] - 1] = d1;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- c_mean_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (row = 0; row < i; row++) {
- mean_img_data[row] = mean_img[b_tmp_data[row] + 1295];
- }
-
- d1 = mean(mean_img_data, c_mean_img_size);
- i = tmp_size[0];
- for (row = 0; row < i; row++) {
- mean_img[tmp_data[row] + 1295] = d1;
- }
-
- /* % */
- memset(&tracked_images[0], 0, 38880000U * sizeof(real_T));
- for (row = 0; row < 1296; row++) {
- tracked_images[row] = mean_img[row];
- tracked_images[12960000 + row] = mean_img[1296 + row];
- tracked_images[25920000 + row] = mean_img[2592 + row];
- }
-
- /* % Affine state particle initialization */
- for (i = 0; i < 25; i++) {
- eye(*(real_T (*)[9])&X_par[9 * i]);
- for (row = 0; row < 2; row++) {
- for (col = 0; col < 2; col++) {
- X_par[(col + 3 * row) + 9 * i] = (real_T)iv0[col + (row << 1)];
- }
- }
-
- eye(*(real_T (*)[9])&AR_velocity[9 * i]);
- }
-
- /* % dw_dp in matrix form %% */
- memset(&dw_dp[0], 0, 15552U * sizeof(real_T));
- i = -1;
- for (col = 0; col < 36; col++) {
- for (row = 0; row < 36; row++) {
- i++;
- dw_dp[12 * i] = mx[row + 36 * col];
- dw_dp[2 + 12 * i] = 0.0;
- dw_dp[4 + 12 * i] = my[row + 36 * col];
- dw_dp[6 + 12 * i] = 0.0;
- dw_dp[8 + 12 * i] = 1.0;
- dw_dp[10 + 12 * i] = 0.0;
- dw_dp[1 + 12 * i] = 0.0;
- dw_dp[3 + 12 * i] = mx[row + 36 * col];
- dw_dp[5 + 12 * i] = 0.0;
- dw_dp[7 + 12 * i] = my[row + 36 * col];
- dw_dp[9 + 12 * i] = 0.0;
- dw_dp[11 + 12 * i] = 1.0;
- }
- }
-}
-
-/* End of code generation (init_variables.cpp) */
diff --git a/.svn/pristine/7e/7ec731130e079636b398c19c12e9b1650683cf8e.svn-base b/.svn/pristine/7e/7ec731130e079636b398c19c12e9b1650683cf8e.svn-base
deleted file mode 100644
index 7a8f3f68fb79124ca8b78916ce7f6010b4b2d04b..0000000000000000000000000000000000000000
--- a/.svn/pristine/7e/7ec731130e079636b398c19c12e9b1650683cf8e.svn-base
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * diag.cpp
- *
- * Code generation for function 'diag'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "diag.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_diag(const creal_T v[2], creal_T d[4])
-{
- int32_T j;
- for (j = 0; j < 4; j++) {
- d[j].re = 0.0;
- d[j].im = 0.0;
- }
-
- for (j = 0; j < 2; j++) {
- d[j + (j << 1)] = v[j];
- }
-}
-
-void diag(const creal_T v[3], creal_T d[9])
-{
- int32_T j;
- for (j = 0; j < 9; j++) {
- d[j].re = 0.0;
- d[j].im = 0.0;
- }
-
- for (j = 0; j < 3; j++) {
- d[j + 3 * j] = v[j];
- }
-}
-
-/* End of code generation (diag.cpp) */
diff --git a/.svn/pristine/81/815f8d867c2289cfe344a68fd0bc8b0e62dd59a4.svn-base b/.svn/pristine/81/815f8d867c2289cfe344a68fd0bc8b0e62dd59a4.svn-base
deleted file mode 100644
index 367ea5bd5367aaee64fe408c58257b86785bd1f6..0000000000000000000000000000000000000000
--- a/.svn/pristine/81/815f8d867c2289cfe344a68fd0bc8b0e62dd59a4.svn-base
+++ /dev/null
@@ -1,2508 +0,0 @@
-/*
- * eig.cpp
- *
- * Code generation for function 'eig'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "eig.h"
-#include "mrdivide.h"
-#include "log.h"
-#include "sqrt.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static creal_T b_eml_div(const creal_T x, real_T y);
-static void b_eml_matlab_zggev(creal_T A[4], real_T *info, creal_T alpha1[2],
- creal_T beta1[2], creal_T V[4]);
-static void b_eml_matlab_zhgeqz(creal_T A[4], int32_T ilo, int32_T ihi, creal_T
- Z[4], real_T *info, creal_T alpha1[2], creal_T beta1[2]);
-static void b_eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
- creal_T *sn);
-static void b_eml_matlab_ztgevc(const creal_T A[4], creal_T V[4]);
-static void eml_matlab_zggev(creal_T A[9], real_T *info, creal_T alpha1[3],
- creal_T beta1[3], creal_T V[9]);
-static void eml_matlab_zhgeqz(creal_T A[9], int32_T ilo, int32_T ihi, creal_T Z
- [9], real_T *info, creal_T alpha1[3], creal_T beta1[3]);
-static void eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
- creal_T *sn, creal_T *r);
-static void eml_matlab_ztgevc(const creal_T A[9], creal_T V[9]);
-
-/* Function Definitions */
-static creal_T b_eml_div(const creal_T x, real_T y)
-{
- creal_T z;
- if (x.im == 0.0) {
- z.re = x.re / y;
- z.im = 0.0;
- } else if (x.re == 0.0) {
- z.re = 0.0;
- z.im = x.im / y;
- } else {
- z.re = x.re / y;
- z.im = x.im / y;
- }
-
- return z;
-}
-
-static void b_eml_matlab_zggev(creal_T A[4], real_T *info, creal_T alpha1[2],
- creal_T beta1[2], creal_T V[4])
-{
- real_T anrm;
- int32_T nzcount;
- boolean_T exitg5;
- real_T absxk;
- int32_T i;
- boolean_T ilascl;
- real_T anrmto;
- real_T ctoc;
- boolean_T notdone;
- real_T cfrom1;
- real_T cto1;
- real_T mul;
- creal_T b_A[4];
- int8_T rscale[2];
- int32_T ilo;
- int32_T ihi;
- int32_T j;
- int32_T ii;
- boolean_T exitg3;
- int32_T jj;
- boolean_T exitg4;
- boolean_T guard2 = FALSE;
- boolean_T exitg1;
- boolean_T exitg2;
- boolean_T guard1 = FALSE;
- static const int8_T iv7[4] = { 1, 0, 0, 1 };
-
- *info = 0.0;
- anrm = 0.0;
- nzcount = 0;
- exitg5 = FALSE;
- while ((exitg5 == FALSE) && (nzcount < 4)) {
- absxk = rt_hypotd_snf(fabs(A[nzcount].re), fabs(A[nzcount].im));
- if (rtIsNaN(absxk)) {
- anrm = rtNaN;
- exitg5 = TRUE;
- } else {
- if (absxk > anrm) {
- anrm = absxk;
- }
-
- nzcount++;
- }
- }
-
- if (!((!rtIsInf(anrm)) && (!rtIsNaN(anrm)))) {
- for (i = 0; i < 2; i++) {
- alpha1[i].re = rtNaN;
- alpha1[i].im = 0.0;
- beta1[i].re = rtNaN;
- beta1[i].im = 0.0;
- }
-
- for (nzcount = 0; nzcount < 4; nzcount++) {
- V[nzcount].re = rtNaN;
- V[nzcount].im = 0.0;
- }
- } else {
- ilascl = FALSE;
- anrmto = anrm;
- if ((anrm > 0.0) && (anrm < 6.7178761075670888E-139)) {
- anrmto = 6.7178761075670888E-139;
- ilascl = TRUE;
- } else {
- if (anrm > 1.4885657073574029E+138) {
- anrmto = 1.4885657073574029E+138;
- ilascl = TRUE;
- }
- }
-
- if (ilascl) {
- absxk = anrm;
- ctoc = anrmto;
- notdone = TRUE;
- while (notdone) {
- cfrom1 = absxk * 2.0041683600089728E-292;
- cto1 = ctoc / 4.9896007738368E+291;
- if ((cfrom1 > ctoc) && (ctoc != 0.0)) {
- mul = 2.0041683600089728E-292;
- absxk = cfrom1;
- } else if (cto1 > absxk) {
- mul = 4.9896007738368E+291;
- ctoc = cto1;
- } else {
- mul = ctoc / absxk;
- notdone = FALSE;
- }
-
- for (nzcount = 0; nzcount < 4; nzcount++) {
- A[nzcount].re *= mul;
- A[nzcount].im *= mul;
- }
- }
- }
-
- memcpy(&b_A[0], &A[0], sizeof(creal_T) << 2);
- for (i = 0; i < 2; i++) {
- rscale[i] = 0;
- }
-
- ilo = 1;
- ihi = 2;
- i = 0;
- j = 0;
- notdone = FALSE;
- ii = 2;
- exitg3 = FALSE;
- while ((exitg3 == FALSE) && (ii > 0)) {
- nzcount = 0;
- i = ii;
- j = 2;
- jj = 1;
- exitg4 = FALSE;
- while ((exitg4 == FALSE) && (jj <= 2)) {
- guard2 = FALSE;
- if ((A[(ii + ((jj - 1) << 1)) - 1].re != 0.0) || (A[(ii + ((jj - 1) << 1))
- - 1].im != 0.0) || (ii == jj)) {
- if (nzcount == 0) {
- j = jj;
- nzcount = 1;
- guard2 = TRUE;
- } else {
- nzcount = 2;
- exitg4 = TRUE;
- }
- } else {
- guard2 = TRUE;
- }
-
- if (guard2 == TRUE) {
- jj++;
- }
- }
-
- if (nzcount < 2) {
- notdone = TRUE;
- exitg3 = TRUE;
- } else {
- ii--;
- }
- }
-
- if (!notdone) {
- i = 0;
- j = 0;
- notdone = FALSE;
- jj = 1;
- exitg1 = FALSE;
- while ((exitg1 == FALSE) && (jj <= 2)) {
- nzcount = 0;
- i = 2;
- j = jj;
- ii = 1;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (ii <= 2)) {
- guard1 = FALSE;
- if ((b_A[(ii + ((jj - 1) << 1)) - 1].re != 0.0) || (b_A[(ii + ((jj - 1)
- << 1)) - 1].im != 0.0) || (ii == jj)) {
- if (nzcount == 0) {
- i = ii;
- nzcount = 1;
- guard1 = TRUE;
- } else {
- nzcount = 2;
- exitg2 = TRUE;
- }
- } else {
- guard1 = TRUE;
- }
-
- if (guard1 == TRUE) {
- ii++;
- }
- }
-
- if (nzcount < 2) {
- notdone = TRUE;
- exitg1 = TRUE;
- } else {
- jj++;
- }
- }
-
- if (!notdone) {
- } else {
- if (i != 1) {
- for (nzcount = 0; nzcount + 1 < 3; nzcount++) {
- absxk = b_A[(i + (nzcount << 1)) - 1].re;
- ctoc = b_A[(i + (nzcount << 1)) - 1].im;
- b_A[(i + (nzcount << 1)) - 1] = b_A[nzcount << 1];
- b_A[nzcount << 1].re = absxk;
- b_A[nzcount << 1].im = ctoc;
- }
- }
-
- if (j != 1) {
- for (nzcount = 0; nzcount < 2; nzcount++) {
- absxk = b_A[nzcount + ((j - 1) << 1)].re;
- ctoc = b_A[nzcount + ((j - 1) << 1)].im;
- b_A[nzcount + ((j - 1) << 1)] = b_A[nzcount];
- b_A[nzcount].re = absxk;
- b_A[nzcount].im = ctoc;
- }
- }
-
- rscale[0] = (int8_T)j;
- ilo = 2;
- rscale[1] = 2;
- }
- } else {
- if (i != 2) {
- for (nzcount = 0; nzcount < 2; nzcount++) {
- absxk = b_A[nzcount << 1].re;
- ctoc = b_A[nzcount << 1].im;
- b_A[nzcount << 1] = b_A[1 + (nzcount << 1)];
- b_A[1 + (nzcount << 1)].re = absxk;
- b_A[1 + (nzcount << 1)].im = ctoc;
- }
- }
-
- if (j != 2) {
- for (nzcount = 0; nzcount < 2; nzcount++) {
- absxk = b_A[nzcount].re;
- ctoc = b_A[nzcount].im;
- b_A[nzcount] = b_A[2 + nzcount];
- b_A[2 + nzcount].re = absxk;
- b_A[2 + nzcount].im = ctoc;
- }
- }
-
- rscale[1] = (int8_T)j;
- ihi = 1;
- rscale[0] = 1;
- }
-
- for (nzcount = 0; nzcount < 4; nzcount++) {
- V[nzcount].re = (real_T)iv7[nzcount];
- V[nzcount].im = 0.0;
- }
-
- b_eml_matlab_zhgeqz(b_A, ilo, ihi, V, info, alpha1, beta1);
- if (*info != 0.0) {
- } else {
- b_eml_matlab_ztgevc(b_A, V);
- if ((ilo > 1) && (rscale[0] != 1)) {
- for (j = 0; j < 2; j++) {
- absxk = V[j << 1].re;
- ctoc = V[j << 1].im;
- V[j << 1] = V[(rscale[0] + (j << 1)) - 1];
- V[(rscale[0] + (j << 1)) - 1].re = absxk;
- V[(rscale[0] + (j << 1)) - 1].im = ctoc;
- }
- }
-
- if ((ihi < 2) && (rscale[1] != 2)) {
- for (j = 0; j < 2; j++) {
- absxk = V[1 + (j << 1)].re;
- ctoc = V[1 + (j << 1)].im;
- V[1 + (j << 1)] = V[j << 1];
- V[(rscale[1] + (j << 1)) - 1].re = absxk;
- V[(rscale[1] + (j << 1)) - 1].im = ctoc;
- }
- }
-
- for (nzcount = 0; nzcount < 2; nzcount++) {
- absxk = fabs(V[1 + (nzcount << 1)].re) + fabs(V[1 + (nzcount << 1)].im);
- ctoc = fabs(V[nzcount << 1].re) + fabs(V[nzcount << 1].im);
- if (absxk > ctoc) {
- ctoc = absxk;
- }
-
- if (ctoc >= 6.7178761075670888E-139) {
- ctoc = 1.0 / ctoc;
- for (ii = 0; ii < 2; ii++) {
- V[ii + (nzcount << 1)].re *= ctoc;
- V[ii + (nzcount << 1)].im *= ctoc;
- }
- }
- }
-
- if (ilascl) {
- notdone = TRUE;
- while (notdone) {
- cfrom1 = anrmto * 2.0041683600089728E-292;
- cto1 = anrm / 4.9896007738368E+291;
- if ((cfrom1 > anrm) && (anrm != 0.0)) {
- mul = 2.0041683600089728E-292;
- anrmto = cfrom1;
- } else if (cto1 > anrmto) {
- mul = 4.9896007738368E+291;
- anrm = cto1;
- } else {
- mul = anrm / anrmto;
- notdone = FALSE;
- }
-
- for (nzcount = 0; nzcount < 2; nzcount++) {
- alpha1[nzcount].re *= mul;
- alpha1[nzcount].im *= mul;
- }
- }
- }
- }
- }
-}
-
-static void b_eml_matlab_zhgeqz(creal_T A[4], int32_T ilo, int32_T ihi, creal_T
- Z[4], real_T *info, creal_T alpha1[2], creal_T beta1[2])
-{
- int32_T i;
- real_T eshift_re;
- real_T eshift_im;
- creal_T ctemp;
- real_T rho_re;
- real_T rho_im;
- real_T anorm;
- real_T scale;
- real_T sumsq;
- boolean_T firstNonZero;
- int32_T j;
- int32_T istart;
- real_T reAij;
- real_T imAij;
- real_T temp2;
- real_T b_atol;
- real_T ascale;
- boolean_T failed;
- boolean_T guard1 = FALSE;
- boolean_T guard2 = FALSE;
- int32_T ifirst;
- int32_T ilast;
- int32_T ilastm1;
- int32_T iiter;
- boolean_T goto60;
- boolean_T goto70;
- boolean_T goto90;
- int32_T jiter;
- int32_T exitg1;
- boolean_T exitg3;
- boolean_T b_guard1 = FALSE;
- creal_T b_A;
- creal_T t1;
- creal_T d;
- real_T sigma1_im;
- boolean_T exitg2;
- creal_T c_A;
- *info = 0.0;
- for (i = 0; i < 2; i++) {
- alpha1[i].re = 0.0;
- alpha1[i].im = 0.0;
- beta1[i].re = 1.0;
- beta1[i].im = 0.0;
- }
-
- eshift_re = 0.0;
- eshift_im = 0.0;
- ctemp.re = 0.0;
- ctemp.im = 0.0;
- rho_re = 0.0;
- rho_im = 0.0;
- anorm = 0.0;
- if (ilo > ihi) {
- } else {
- scale = 0.0;
- sumsq = 0.0;
- firstNonZero = TRUE;
- for (j = ilo; j <= ihi; j++) {
- istart = j + 1;
- if (ihi < j + 1) {
- istart = ihi;
- }
-
- for (i = ilo; i <= istart; i++) {
- reAij = A[(i + ((j - 1) << 1)) - 1].re;
- imAij = A[(i + ((j - 1) << 1)) - 1].im;
- if (reAij != 0.0) {
- anorm = fabs(reAij);
- if (firstNonZero) {
- sumsq = 1.0;
- scale = anorm;
- firstNonZero = FALSE;
- } else if (scale < anorm) {
- temp2 = scale / anorm;
- sumsq = 1.0 + sumsq * temp2 * temp2;
- scale = anorm;
- } else {
- temp2 = anorm / scale;
- sumsq += temp2 * temp2;
- }
- }
-
- if (imAij != 0.0) {
- anorm = fabs(imAij);
- if (firstNonZero) {
- sumsq = 1.0;
- scale = anorm;
- firstNonZero = FALSE;
- } else if (scale < anorm) {
- temp2 = scale / anorm;
- sumsq = 1.0 + sumsq * temp2 * temp2;
- scale = anorm;
- } else {
- temp2 = anorm / scale;
- sumsq += temp2 * temp2;
- }
- }
- }
- }
-
- anorm = scale * sqrt(sumsq);
- }
-
- reAij = 2.2204460492503131E-16 * anorm;
- b_atol = 2.2250738585072014E-308;
- if (reAij > 2.2250738585072014E-308) {
- b_atol = reAij;
- }
-
- reAij = 2.2250738585072014E-308;
- if (anorm > 2.2250738585072014E-308) {
- reAij = anorm;
- }
-
- ascale = 1.0 / reAij;
- failed = TRUE;
- j = ihi + 1;
- while (j < 3) {
- alpha1[1] = A[3];
- j = 3;
- }
-
- guard1 = FALSE;
- guard2 = FALSE;
- if (ihi >= ilo) {
- ifirst = ilo;
- istart = ilo;
- ilast = ihi - 1;
- ilastm1 = ihi - 2;
- iiter = 0;
- goto60 = FALSE;
- goto70 = FALSE;
- goto90 = FALSE;
- jiter = 1;
- do {
- exitg1 = 0;
- if (jiter <= 30 * ((ihi - ilo) + 1)) {
- if (ilast + 1 == ilo) {
- goto60 = TRUE;
- } else if (fabs(A[ilast + (ilastm1 << 1)].re) + fabs(A[ilast + (ilastm1 <<
- 1)].im) <= b_atol) {
- A[ilast + (ilastm1 << 1)].re = 0.0;
- A[ilast + (ilastm1 << 1)].im = 0.0;
- goto60 = TRUE;
- } else {
- j = ilastm1;
- exitg3 = FALSE;
- while ((exitg3 == FALSE) && (j + 1 >= ilo)) {
- if (j + 1 == ilo) {
- firstNonZero = TRUE;
- } else if (fabs(A[j].re) + fabs(A[j].im) <= b_atol) {
- A[j].re = 0.0;
- A[j].im = 0.0;
- firstNonZero = TRUE;
- } else {
- firstNonZero = FALSE;
- }
-
- if (firstNonZero) {
- ifirst = j + 1;
- goto70 = TRUE;
- exitg3 = TRUE;
- } else {
- j--;
- }
- }
- }
-
- if (goto60 || goto70) {
- firstNonZero = TRUE;
- } else {
- firstNonZero = FALSE;
- }
-
- if (!firstNonZero) {
- for (i = 0; i < 2; i++) {
- alpha1[i].re = rtNaN;
- alpha1[i].im = 0.0;
- beta1[i].re = rtNaN;
- beta1[i].im = 0.0;
- }
-
- for (istart = 0; istart < 2; istart++) {
- for (ifirst = 0; ifirst < 2; ifirst++) {
- Z[ifirst + (istart << 1)].re = rtNaN;
- Z[ifirst + (istart << 1)].im = 0.0;
- }
- }
-
- *info = -1.0;
- exitg1 = 1;
- } else {
- b_guard1 = FALSE;
- if (goto60) {
- goto60 = FALSE;
- alpha1[ilast] = A[ilast + (ilast << 1)];
- ilast = ilastm1;
- ilastm1--;
- if (ilast + 1 < ilo) {
- failed = FALSE;
- guard2 = TRUE;
- exitg1 = 1;
- } else {
- iiter = 0;
- eshift_re = 0.0;
- eshift_im = 0.0;
- b_guard1 = TRUE;
- }
- } else {
- if (goto70) {
- goto70 = FALSE;
- iiter++;
- if (iiter - iiter / 10 * 10 != 0) {
- b_A.re = -(A[ilast + (ilast << 1)].re - A[ilastm1 + (ilastm1 <<
- 1)].re);
- b_A.im = -(A[ilast + (ilast << 1)].im - A[ilastm1 + (ilastm1 <<
- 1)].im);
- t1 = b_eml_div(b_A, 2.0);
- anorm = A[ilastm1 + (ilast << 1)].re * A[ilast + (ilastm1 << 1)]
- .re - A[ilastm1 + (ilast << 1)].im * A[ilast + (ilastm1 << 1)]
- .im;
- reAij = A[ilastm1 + (ilast << 1)].re * A[ilast + (ilastm1 << 1)]
- .im + A[ilastm1 + (ilast << 1)].im * A[ilast + (ilastm1 << 1)]
- .re;
- d.re = (t1.re * t1.re - t1.im * t1.im) + anorm;
- d.im = (t1.re * t1.im + t1.im * t1.re) + reAij;
- b_sqrt(&d);
- temp2 = A[ilastm1 + (ilastm1 << 1)].re - (t1.re - d.re);
- sigma1_im = A[ilastm1 + (ilastm1 << 1)].im - (t1.im - d.im);
- scale = A[ilastm1 + (ilastm1 << 1)].re - (t1.re + d.re);
- sumsq = A[ilastm1 + (ilastm1 << 1)].im - (t1.im + d.im);
- rho_re = temp2 - A[ilast + (ilast << 1)].re;
- rho_im = sigma1_im - A[ilast + (ilast << 1)].im;
- anorm = scale - A[ilast + (ilast << 1)].re;
- reAij = sumsq - A[ilast + (ilast << 1)].im;
- if (rt_hypotd_snf(fabs(rho_re), fabs(rho_im)) <= rt_hypotd_snf
- (fabs(anorm), fabs(reAij))) {
- scale = temp2;
- sumsq = sigma1_im;
- rho_re = t1.re - d.re;
- rho_im = t1.im - d.im;
- } else {
- rho_re = t1.re + d.re;
- rho_im = t1.im + d.im;
- }
- } else {
- eshift_re += A[ilast + (ilastm1 << 1)].re;
- eshift_im += A[ilast + (ilastm1 << 1)].im;
- scale = eshift_re;
- sumsq = eshift_im;
- }
-
- j = ilastm1;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (j + 1 > ifirst)) {
- istart = 2;
- ctemp.re = A[3].re - scale;
- ctemp.im = A[3].im - sumsq;
- anorm = ascale * (fabs(ctemp.re) + fabs(ctemp.im));
- temp2 = ascale * (fabs(A[3].re) + fabs(A[3].im));
- reAij = anorm;
- if (temp2 > anorm) {
- reAij = temp2;
- }
-
- if ((reAij < 1.0) && (reAij != 0.0)) {
- anorm /= reAij;
- temp2 /= reAij;
- }
-
- if ((fabs(A[1].re) + fabs(A[1].im)) * temp2 <= anorm * b_atol) {
- goto90 = TRUE;
- exitg2 = TRUE;
- } else {
- j = 0;
- }
- }
-
- if (!goto90) {
- istart = ifirst;
- if (ifirst == ilastm1 + 1) {
- ctemp.re = rho_re;
- ctemp.im = rho_im;
- } else {
- ctemp.re = A[(ifirst + ((ifirst - 1) << 1)) - 1].re - scale;
- ctemp.im = A[(ifirst + ((ifirst - 1) << 1)) - 1].im - sumsq;
- }
-
- goto90 = TRUE;
- }
- }
-
- if (goto90) {
- goto90 = FALSE;
- c_A = A[1 + ((istart - 1) << 1)];
- b_eml_matlab_zlartg(ctemp, c_A, &imAij, &t1);
- j = istart;
- while (j < ilast + 1) {
- for (j = 0; j < 2; j++) {
- d.re = imAij * A[j << 1].re;
- d.im = imAij * A[j << 1].im;
- temp2 = t1.re * A[1 + (j << 1)].re - t1.im * A[1 + (j << 1)].
- im;
- sigma1_im = t1.re * A[1 + (j << 1)].im + t1.im * A[1 + (j << 1)]
- .re;
- anorm = A[j << 1].re;
- reAij = A[j << 1].im;
- scale = A[j << 1].im;
- sumsq = A[j << 1].re;
- A[1 + (j << 1)].re = imAij * A[1 + (j << 1)].re - (t1.re *
- anorm + t1.im * reAij);
- A[1 + (j << 1)].im = imAij * A[1 + (j << 1)].im - (t1.re *
- scale - t1.im * sumsq);
- A[j << 1].re = d.re + temp2;
- A[j << 1].im = d.im + sigma1_im;
- }
-
- t1.re = -t1.re;
- t1.im = -t1.im;
- for (i = 0; i < 2; i++) {
- d.re = imAij * A[2 + i].re;
- d.im = imAij * A[2 + i].im;
- temp2 = t1.re * A[i].re - t1.im * A[i].im;
- sigma1_im = t1.re * A[i].im + t1.im * A[i].re;
- anorm = A[2 + i].re;
- reAij = A[2 + i].im;
- scale = A[2 + i].im;
- sumsq = A[2 + i].re;
- A[i].re = imAij * A[i].re - (t1.re * anorm + t1.im * reAij);
- A[i].im = imAij * A[i].im - (t1.re * scale - t1.im * sumsq);
- A[2 + i].re = d.re + temp2;
- A[2 + i].im = d.im + sigma1_im;
- }
-
- for (i = 0; i < 2; i++) {
- d.re = imAij * Z[2 + i].re;
- d.im = imAij * Z[2 + i].im;
- temp2 = t1.re * Z[i].re - t1.im * Z[i].im;
- sigma1_im = t1.re * Z[i].im + t1.im * Z[i].re;
- anorm = Z[2 + i].re;
- reAij = Z[2 + i].im;
- scale = Z[2 + i].im;
- sumsq = Z[2 + i].re;
- Z[i].re = imAij * Z[i].re - (t1.re * anorm + t1.im * reAij);
- Z[i].im = imAij * Z[i].im - (t1.re * scale - t1.im * sumsq);
- Z[2 + i].re = d.re + temp2;
- Z[2 + i].im = d.im + sigma1_im;
- }
-
- j = 2;
- }
- }
-
- b_guard1 = TRUE;
- }
-
- if (b_guard1 == TRUE) {
- jiter++;
- }
- }
- } else {
- guard2 = TRUE;
- exitg1 = 1;
- }
- } while (exitg1 == 0);
- } else {
- guard1 = TRUE;
- }
-
- if (guard2 == TRUE) {
- if (failed) {
- *info = (real_T)(ilast + 1);
- for (ifirst = 0; ifirst + 1 <= ilast + 1; ifirst++) {
- alpha1[ifirst].re = rtNaN;
- alpha1[ifirst].im = 0.0;
- beta1[ifirst].re = rtNaN;
- beta1[ifirst].im = 0.0;
- }
-
- for (istart = 0; istart < 2; istart++) {
- for (ifirst = 0; ifirst < 2; ifirst++) {
- Z[ifirst + (istart << 1)].re = rtNaN;
- Z[ifirst + (istart << 1)].im = 0.0;
- }
- }
- } else {
- guard1 = TRUE;
- }
- }
-
- if (guard1 == TRUE) {
- for (j = 0; j + 1 < ilo; j++) {
- alpha1[j] = A[j + (j << 1)];
- }
- }
-}
-
-static void b_eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
- creal_T *sn)
-{
- real_T scale;
- real_T f2s;
- real_T g2;
- real_T fs_re;
- real_T fs_im;
- real_T gs_re;
- real_T gs_im;
- boolean_T guard1 = FALSE;
- real_T g2s;
- scale = fabs(f.re);
- f2s = fabs(f.im);
- if (f2s > scale) {
- scale = f2s;
- }
-
- f2s = fabs(g.re);
- g2 = fabs(g.im);
- if (g2 > f2s) {
- f2s = g2;
- }
-
- if (f2s > scale) {
- scale = f2s;
- }
-
- fs_re = f.re;
- fs_im = f.im;
- gs_re = g.re;
- gs_im = g.im;
- guard1 = FALSE;
- if (scale >= 7.4428285367870146E+137) {
- do {
- fs_re *= 1.3435752215134178E-138;
- fs_im *= 1.3435752215134178E-138;
- gs_re *= 1.3435752215134178E-138;
- gs_im *= 1.3435752215134178E-138;
- scale *= 1.3435752215134178E-138;
- } while (!(scale < 7.4428285367870146E+137));
-
- guard1 = TRUE;
- } else if (scale <= 1.3435752215134178E-138) {
- if ((g.re == 0.0) && (g.im == 0.0)) {
- *cs = 1.0;
- sn->re = 0.0;
- sn->im = 0.0;
- } else {
- do {
- fs_re *= 7.4428285367870146E+137;
- fs_im *= 7.4428285367870146E+137;
- gs_re *= 7.4428285367870146E+137;
- gs_im *= 7.4428285367870146E+137;
- scale *= 7.4428285367870146E+137;
- } while (!(scale > 1.3435752215134178E-138));
-
- guard1 = TRUE;
- }
- } else {
- guard1 = TRUE;
- }
-
- if (guard1 == TRUE) {
- scale = fs_re * fs_re + fs_im * fs_im;
- g2 = gs_re * gs_re + gs_im * gs_im;
- f2s = g2;
- if (1.0 > g2) {
- f2s = 1.0;
- }
-
- if (scale <= f2s * 2.0041683600089728E-292) {
- if ((f.re == 0.0) && (f.im == 0.0)) {
- *cs = 0.0;
- scale = rt_hypotd_snf(fabs(gs_re), fabs(gs_im));
- sn->re = gs_re / scale;
- sn->im = -gs_im / scale;
- } else {
- g2s = sqrt(g2);
- *cs = rt_hypotd_snf(fabs(fs_re), fabs(fs_im)) / g2s;
- f2s = fabs(f.re);
- g2 = fabs(f.im);
- if (g2 > f2s) {
- f2s = g2;
- }
-
- if (f2s > 1.0) {
- scale = rt_hypotd_snf(fabs(f.re), fabs(f.im));
- fs_re = f.re / scale;
- fs_im = f.im / scale;
- } else {
- f2s = 7.4428285367870146E+137 * f.re;
- g2 = 7.4428285367870146E+137 * f.im;
- scale = rt_hypotd_snf(fabs(f2s), fabs(g2));
- fs_re = f2s / scale;
- fs_im = g2 / scale;
- }
-
- gs_re /= g2s;
- gs_im = -gs_im / g2s;
- sn->re = fs_re * gs_re - fs_im * gs_im;
- sn->im = fs_re * gs_im + fs_im * gs_re;
- }
- } else {
- f2s = sqrt(1.0 + g2 / scale);
- *cs = 1.0 / f2s;
- scale += g2;
- fs_re = f2s * fs_re / scale;
- fs_im = f2s * fs_im / scale;
- sn->re = fs_re * gs_re - fs_im * -gs_im;
- sn->im = fs_re * -gs_im + fs_im * gs_re;
- }
- }
-}
-
-static void b_eml_matlab_ztgevc(const creal_T A[4], creal_T V[4])
-{
- creal_T work1[2];
- creal_T work2[2];
- int32_T i;
- real_T y;
- real_T anorm;
- real_T dmin;
- real_T ascale;
- int32_T je;
- real_T temp;
- real_T salpha_re;
- real_T salpha_im;
- real_T acoeff;
- boolean_T b2;
- boolean_T b3;
- real_T scale;
- real_T b_y;
- creal_T d;
- creal_T b_work1;
- int32_T jc;
- for (i = 0; i < 2; i++) {
- work1[i].re = 0.0;
- work1[i].im = 0.0;
- work2[i].re = 0.0;
- work2[i].im = 0.0;
- }
-
- y = (fabs(A[2].re) + fabs(A[2].im)) + (fabs(A[3].re) + fabs(A[3].im));
- anorm = fabs(A[0].re) + fabs(A[0].im);
- if (y > anorm) {
- anorm = y;
- }
-
- dmin = anorm;
- if (2.2250738585072014E-308 > anorm) {
- dmin = 2.2250738585072014E-308;
- }
-
- ascale = 1.0 / dmin;
- for (je = 0; je < 2; je++) {
- y = (fabs(A[(((1 - je) << 1) - je) + 1].re) + fabs(A[(((1 - je) << 1) - je)
- + 1].im)) * ascale;
- if (1.0 > y) {
- y = 1.0;
- }
-
- temp = 1.0 / y;
- salpha_re = ascale * (temp * A[(((1 - je) << 1) - je) + 1].re);
- salpha_im = ascale * (temp * A[(((1 - je) << 1) - je) + 1].im);
- acoeff = temp * ascale;
- if ((fabs(temp) >= 2.2250738585072014E-308) && (fabs(acoeff) <
- 2.0041683600089728E-292)) {
- b2 = TRUE;
- } else {
- b2 = FALSE;
- }
-
- if ((fabs(salpha_re) + fabs(salpha_im) >= 2.2250738585072014E-308) && (fabs
- (salpha_re) + fabs(salpha_im) < 2.0041683600089728E-292)) {
- b3 = TRUE;
- } else {
- b3 = FALSE;
- }
-
- scale = 1.0;
- if (b2) {
- dmin = anorm;
- if (4.9896007738368E+291 < anorm) {
- dmin = 4.9896007738368E+291;
- }
-
- scale = 2.0041683600089728E-292 / fabs(temp) * dmin;
- }
-
- if (b3) {
- dmin = 2.0041683600089728E-292 / (fabs(salpha_re) + fabs(salpha_im));
- if (dmin > scale) {
- scale = dmin;
- }
- }
-
- if (b2 || b3) {
- y = fabs(acoeff);
- b_y = fabs(salpha_re) + fabs(salpha_im);
- if (1.0 > y) {
- y = 1.0;
- }
-
- if (b_y > y) {
- y = b_y;
- }
-
- y = 1.0 / (2.2250738585072014E-308 * y);
- if (y < scale) {
- scale = y;
- }
-
- if (b2) {
- acoeff = ascale * (scale * temp);
- } else {
- acoeff *= scale;
- }
-
- if (b3) {
- salpha_re *= scale;
- salpha_im *= scale;
- } else {
- salpha_re *= scale;
- salpha_im *= scale;
- }
- }
-
- for (i = 0; i < 2; i++) {
- work1[i].re = 0.0;
- work1[i].im = 0.0;
- }
-
- work1[1 - je].re = 1.0;
- work1[1 - je].im = 0.0;
- dmin = 2.2204460492503131E-16 * fabs(acoeff) * anorm;
- y = 2.2204460492503131E-16 * (fabs(salpha_re) + fabs(salpha_im));
- if (y > dmin) {
- dmin = y;
- }
-
- if (2.2250738585072014E-308 > dmin) {
- dmin = 2.2250738585072014E-308;
- }
-
- i = 0;
- while (i <= -je) {
- work1[0].re = acoeff * A[(1 - je) << 1].re;
- work1[0].im = acoeff * A[(1 - je) << 1].im;
- i = 1;
- }
-
- work1[1 - je].re = 1.0;
- work1[1 - je].im = 0.0;
- i = 0;
- while (i <= -je) {
- d.re = acoeff * A[0].re - salpha_re;
- d.im = acoeff * A[0].im - salpha_im;
- if (fabs(d.re) + fabs(d.im) <= dmin) {
- d.re = dmin;
- d.im = 0.0;
- }
-
- if ((fabs(d.re) + fabs(d.im) < 1.0) && (fabs(work1[0].re) + fabs(work1[0].
- im) >= 2.2471164185778949E+307 * (fabs(d.re) + fabs(d.im)))) {
- temp = 1.0 / (fabs(work1[0].re) + fabs(work1[0].im));
- for (i = 0; i <= 1 - je; i++) {
- work1[i].re *= temp;
- work1[i].im *= temp;
- }
- }
-
- b_work1.re = -work1[0].re;
- b_work1.im = -work1[0].im;
- work1[0] = c_eml_div(b_work1, d);
- i = 1;
- }
-
- for (i = 0; i < 2; i++) {
- work2[i].re = 0.0;
- work2[i].im = 0.0;
- }
-
- for (jc = 0; jc <= 1 - je; jc++) {
- for (i = 0; i < 2; i++) {
- dmin = V[i + (jc << 1)].re * work1[jc].re - V[i + (jc << 1)].im *
- work1[jc].im;
- b_y = V[i + (jc << 1)].re * work1[jc].im + V[i + (jc << 1)].im *
- work1[jc].re;
- work2[i].re += dmin;
- work2[i].im += b_y;
- }
- }
-
- y = fabs(work2[1].re) + fabs(work2[1].im);
- dmin = fabs(work2[0].re) + fabs(work2[0].im);
- if (y > dmin) {
- dmin = y;
- }
-
- if (dmin > 2.2250738585072014E-308) {
- temp = 1.0 / dmin;
- for (i = 0; i < 2; i++) {
- V[i + ((1 - je) << 1)].re = temp * work2[i].re;
- V[i + ((1 - je) << 1)].im = temp * work2[i].im;
- }
- } else {
- for (i = 0; i < 2; i++) {
- V[i + ((1 - je) << 1)].re = 0.0;
- V[i + ((1 - je) << 1)].im = 0.0;
- }
- }
- }
-}
-
-static void eml_matlab_zggev(creal_T A[9], real_T *info, creal_T alpha1[3],
- creal_T beta1[3], creal_T V[9])
-{
- real_T anrm;
- int32_T nzcount;
- boolean_T exitg7;
- real_T absxk;
- int32_T i;
- int32_T ii;
- boolean_T ilascl;
- real_T anrmto;
- real_T ctoc;
- boolean_T notdone;
- real_T cfrom1;
- real_T cto1;
- real_T mul;
- creal_T b_A[9];
- int32_T rscale[3];
- int32_T ilo;
- int32_T ihi;
- int32_T exitg2;
- int32_T j;
- boolean_T exitg5;
- int32_T jj;
- boolean_T exitg6;
- boolean_T guard2 = FALSE;
- creal_T atmp;
- int32_T exitg1;
- boolean_T exitg3;
- boolean_T exitg4;
- boolean_T guard1 = FALSE;
- static const int8_T iv4[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
-
- creal_T c_A;
- creal_T d_A;
- real_T y_re;
- real_T y_im;
- *info = 0.0;
- anrm = 0.0;
- nzcount = 0;
- exitg7 = FALSE;
- while ((exitg7 == FALSE) && (nzcount < 9)) {
- absxk = rt_hypotd_snf(fabs(A[nzcount].re), fabs(A[nzcount].im));
- if (rtIsNaN(absxk)) {
- anrm = rtNaN;
- exitg7 = TRUE;
- } else {
- if (absxk > anrm) {
- anrm = absxk;
- }
-
- nzcount++;
- }
- }
-
- if (!((!rtIsInf(anrm)) && (!rtIsNaN(anrm)))) {
- for (i = 0; i < 3; i++) {
- alpha1[i].re = rtNaN;
- alpha1[i].im = 0.0;
- beta1[i].re = rtNaN;
- beta1[i].im = 0.0;
- }
-
- for (ii = 0; ii < 9; ii++) {
- V[ii].re = rtNaN;
- V[ii].im = 0.0;
- }
- } else {
- ilascl = FALSE;
- anrmto = anrm;
- if ((anrm > 0.0) && (anrm < 6.7178761075670888E-139)) {
- anrmto = 6.7178761075670888E-139;
- ilascl = TRUE;
- } else {
- if (anrm > 1.4885657073574029E+138) {
- anrmto = 1.4885657073574029E+138;
- ilascl = TRUE;
- }
- }
-
- if (ilascl) {
- absxk = anrm;
- ctoc = anrmto;
- notdone = TRUE;
- while (notdone) {
- cfrom1 = absxk * 2.0041683600089728E-292;
- cto1 = ctoc / 4.9896007738368E+291;
- if ((cfrom1 > ctoc) && (ctoc != 0.0)) {
- mul = 2.0041683600089728E-292;
- absxk = cfrom1;
- } else if (cto1 > absxk) {
- mul = 4.9896007738368E+291;
- ctoc = cto1;
- } else {
- mul = ctoc / absxk;
- notdone = FALSE;
- }
-
- for (ii = 0; ii < 9; ii++) {
- A[ii].re *= mul;
- A[ii].im *= mul;
- }
- }
- }
-
- memcpy(&b_A[0], &A[0], 9U * sizeof(creal_T));
- for (i = 0; i < 3; i++) {
- rscale[i] = 0;
- }
-
- ilo = 1;
- ihi = 3;
- do {
- exitg2 = 0;
- i = 0;
- j = 0;
- notdone = FALSE;
- ii = ihi;
- exitg5 = FALSE;
- while ((exitg5 == FALSE) && (ii > 0)) {
- nzcount = 0;
- i = ii;
- j = ihi;
- jj = 1;
- exitg6 = FALSE;
- while ((exitg6 == FALSE) && (jj <= ihi)) {
- guard2 = FALSE;
- if ((b_A[(ii + 3 * (jj - 1)) - 1].re != 0.0) || (b_A[(ii + 3 * (jj - 1))
- - 1].im != 0.0) || (ii == jj)) {
- if (nzcount == 0) {
- j = jj;
- nzcount = 1;
- guard2 = TRUE;
- } else {
- nzcount = 2;
- exitg6 = TRUE;
- }
- } else {
- guard2 = TRUE;
- }
-
- if (guard2 == TRUE) {
- jj++;
- }
- }
-
- if (nzcount < 2) {
- notdone = TRUE;
- exitg5 = TRUE;
- } else {
- ii--;
- }
- }
-
- if (!notdone) {
- exitg2 = 2;
- } else {
- if (i != ihi) {
- for (nzcount = 0; nzcount < 3; nzcount++) {
- atmp = b_A[(i + 3 * nzcount) - 1];
- b_A[(i + 3 * nzcount) - 1] = b_A[(ihi + 3 * nzcount) - 1];
- b_A[(ihi + 3 * nzcount) - 1] = atmp;
- }
- }
-
- if (j != ihi) {
- for (nzcount = 0; nzcount + 1 <= ihi; nzcount++) {
- atmp = b_A[nzcount + 3 * (j - 1)];
- b_A[nzcount + 3 * (j - 1)] = b_A[nzcount + 3 * (ihi - 1)];
- b_A[nzcount + 3 * (ihi - 1)] = atmp;
- }
- }
-
- rscale[ihi - 1] = j;
- ihi--;
- if (ihi == 1) {
- rscale[0] = 1;
- exitg2 = 1;
- }
- }
- } while (exitg2 == 0);
-
- if (exitg2 == 1) {
- } else {
- do {
- exitg1 = 0;
- i = 0;
- j = 0;
- notdone = FALSE;
- jj = ilo;
- exitg3 = FALSE;
- while ((exitg3 == FALSE) && (jj <= ihi)) {
- nzcount = 0;
- i = ihi;
- j = jj;
- ii = ilo;
- exitg4 = FALSE;
- while ((exitg4 == FALSE) && (ii <= ihi)) {
- guard1 = FALSE;
- if ((b_A[(ii + 3 * (jj - 1)) - 1].re != 0.0) || (b_A[(ii + 3 * (jj -
- 1)) - 1].im != 0.0) || (ii == jj)) {
- if (nzcount == 0) {
- i = ii;
- nzcount = 1;
- guard1 = TRUE;
- } else {
- nzcount = 2;
- exitg4 = TRUE;
- }
- } else {
- guard1 = TRUE;
- }
-
- if (guard1 == TRUE) {
- ii++;
- }
- }
-
- if (nzcount < 2) {
- notdone = TRUE;
- exitg3 = TRUE;
- } else {
- jj++;
- }
- }
-
- if (!notdone) {
- exitg1 = 1;
- } else {
- if (i != ilo) {
- for (nzcount = ilo - 1; nzcount + 1 < 4; nzcount++) {
- atmp = b_A[(i + 3 * nzcount) - 1];
- b_A[(i + 3 * nzcount) - 1] = b_A[(ilo + 3 * nzcount) - 1];
- b_A[(ilo + 3 * nzcount) - 1] = atmp;
- }
- }
-
- if (j != ilo) {
- for (nzcount = 0; nzcount + 1 <= ihi; nzcount++) {
- atmp = b_A[nzcount + 3 * (j - 1)];
- b_A[nzcount + 3 * (j - 1)] = b_A[nzcount + 3 * (ilo - 1)];
- b_A[nzcount + 3 * (ilo - 1)] = atmp;
- }
- }
-
- rscale[ilo - 1] = j;
- ilo++;
- if (ilo == ihi) {
- rscale[ilo - 1] = ilo;
- exitg1 = 1;
- }
- }
- } while (exitg1 == 0);
- }
-
- for (ii = 0; ii < 9; ii++) {
- V[ii].re = (real_T)iv4[ii];
- V[ii].im = 0.0;
- }
-
- if (ihi < ilo + 2) {
- } else {
- ii = ilo;
- while (ii < 2) {
- c_A = b_A[1];
- d_A = b_A[2];
- eml_matlab_zlartg(c_A, d_A, &cfrom1, &atmp, &b_A[1]);
- b_A[2].re = 0.0;
- b_A[2].im = 0.0;
- for (j = 0; j < 2; j++) {
- cto1 = cfrom1 * b_A[1 + 3 * (j + 1)].re;
- mul = cfrom1 * b_A[1 + 3 * (j + 1)].im;
- y_re = atmp.re * b_A[2 + 3 * (j + 1)].re - atmp.im * b_A[2 + 3 * (j +
- 1)].im;
- y_im = atmp.re * b_A[2 + 3 * (j + 1)].im + atmp.im * b_A[2 + 3 * (j +
- 1)].re;
- absxk = b_A[1 + 3 * (j + 1)].im;
- ctoc = b_A[1 + 3 * (j + 1)].re;
- b_A[2 + 3 * (j + 1)].re = cfrom1 * b_A[2 + 3 * (j + 1)].re - (atmp.re *
- b_A[1 + 3 * (j + 1)].re + atmp.im * b_A[1 + 3 * (j + 1)].im);
- b_A[2 + 3 * (j + 1)].im = cfrom1 * b_A[2 + 3 * (j + 1)].im - (atmp.re *
- absxk - atmp.im * ctoc);
- b_A[1 + 3 * (j + 1)].re = cto1 + y_re;
- b_A[1 + 3 * (j + 1)].im = mul + y_im;
- }
-
- atmp.re = -atmp.re;
- atmp.im = -atmp.im;
- for (i = ilo - 1; i + 1 < 4; i++) {
- cto1 = cfrom1 * b_A[6 + i].re;
- mul = cfrom1 * b_A[6 + i].im;
- y_re = atmp.re * b_A[3 + i].re - atmp.im * b_A[3 + i].im;
- y_im = atmp.re * b_A[3 + i].im + atmp.im * b_A[3 + i].re;
- absxk = b_A[6 + i].im;
- ctoc = b_A[6 + i].re;
- b_A[3 + i].re = cfrom1 * b_A[3 + i].re - (atmp.re * b_A[6 + i].re +
- atmp.im * b_A[6 + i].im);
- b_A[3 + i].im = cfrom1 * b_A[3 + i].im - (atmp.re * absxk - atmp.im *
- ctoc);
- b_A[6 + i].re = cto1 + y_re;
- b_A[6 + i].im = mul + y_im;
- }
-
- for (i = 0; i < 3; i++) {
- cto1 = cfrom1 * V[6 + i].re;
- mul = cfrom1 * V[6 + i].im;
- y_re = atmp.re * V[3 + i].re - atmp.im * V[3 + i].im;
- y_im = atmp.re * V[3 + i].im + atmp.im * V[3 + i].re;
- absxk = V[6 + i].im;
- ctoc = V[6 + i].re;
- V[3 + i].re = cfrom1 * V[3 + i].re - (atmp.re * V[6 + i].re + atmp.im *
- V[6 + i].im);
- V[3 + i].im = cfrom1 * V[3 + i].im - (atmp.re * absxk - atmp.im * ctoc);
- V[6 + i].re = cto1 + y_re;
- V[6 + i].im = mul + y_im;
- }
-
- ii = 2;
- }
- }
-
- eml_matlab_zhgeqz(b_A, ilo, ihi, V, info, alpha1, beta1);
- if (*info != 0.0) {
- } else {
- eml_matlab_ztgevc(b_A, V);
- if (ilo > 1) {
- for (i = ilo - 2; i + 1 >= 1; i--) {
- if (rscale[i] != i + 1) {
- for (j = 0; j < 3; j++) {
- atmp = V[i + 3 * j];
- V[i + 3 * j] = V[(rscale[i] + 3 * j) - 1];
- V[(rscale[i] + 3 * j) - 1] = atmp;
- }
- }
- }
- }
-
- if (ihi < 3) {
- while (ihi + 1 < 4) {
- if (rscale[ihi] != ihi + 1) {
- for (j = 0; j < 3; j++) {
- atmp = V[ihi + 3 * j];
- V[ihi + 3 * j] = V[(rscale[ihi] + 3 * j) - 1];
- V[(rscale[ihi] + 3 * j) - 1] = atmp;
- }
- }
-
- ihi++;
- }
- }
-
- for (ii = 0; ii < 3; ii++) {
- absxk = fabs(V[3 * ii].re) + fabs(V[3 * ii].im);
- for (nzcount = 0; nzcount < 2; nzcount++) {
- ctoc = fabs(V[(nzcount + 3 * ii) + 1].re) + fabs(V[(nzcount + 3 * ii)
- + 1].im);
- if (ctoc > absxk) {
- absxk = ctoc;
- }
- }
-
- if (absxk >= 6.7178761075670888E-139) {
- absxk = 1.0 / absxk;
- for (nzcount = 0; nzcount < 3; nzcount++) {
- V[nzcount + 3 * ii].re *= absxk;
- V[nzcount + 3 * ii].im *= absxk;
- }
- }
- }
-
- if (ilascl) {
- notdone = TRUE;
- while (notdone) {
- cfrom1 = anrmto * 2.0041683600089728E-292;
- cto1 = anrm / 4.9896007738368E+291;
- if ((cfrom1 > anrm) && (anrm != 0.0)) {
- mul = 2.0041683600089728E-292;
- anrmto = cfrom1;
- } else if (cto1 > anrmto) {
- mul = 4.9896007738368E+291;
- anrm = cto1;
- } else {
- mul = anrm / anrmto;
- notdone = FALSE;
- }
-
- for (ii = 0; ii < 3; ii++) {
- alpha1[ii].re *= mul;
- alpha1[ii].im *= mul;
- }
- }
- }
- }
- }
-}
-
-static void eml_matlab_zhgeqz(creal_T A[9], int32_T ilo, int32_T ihi, creal_T Z
- [9], real_T *info, creal_T alpha1[3], creal_T beta1[3])
-{
- int32_T i;
- real_T eshift_re;
- real_T eshift_im;
- creal_T ctemp;
- real_T rho_re;
- real_T rho_im;
- real_T anorm;
- real_T scale;
- real_T sumsq;
- boolean_T firstNonZero;
- int32_T j;
- int32_T jm1;
- real_T reAij;
- real_T imAij;
- real_T temp2;
- real_T b_atol;
- real_T ascale;
- boolean_T failed;
- boolean_T guard1 = FALSE;
- boolean_T guard2 = FALSE;
- int32_T ifirst;
- int32_T istart;
- int32_T ilast;
- int32_T ilastm1;
- int32_T iiter;
- boolean_T goto60;
- boolean_T goto70;
- boolean_T goto90;
- int32_T jiter;
- int32_T exitg1;
- boolean_T exitg3;
- int32_T jp1;
- boolean_T b_guard1 = FALSE;
- creal_T b_A;
- creal_T t1;
- creal_T d;
- real_T sigma1_im;
- boolean_T exitg2;
- creal_T c_A;
- creal_T d_A;
- creal_T dc0;
- *info = 0.0;
- for (i = 0; i < 3; i++) {
- alpha1[i].re = 0.0;
- alpha1[i].im = 0.0;
- beta1[i].re = 1.0;
- beta1[i].im = 0.0;
- }
-
- eshift_re = 0.0;
- eshift_im = 0.0;
- ctemp.re = 0.0;
- ctemp.im = 0.0;
- rho_re = 0.0;
- rho_im = 0.0;
- anorm = 0.0;
- if (ilo > ihi) {
- } else {
- scale = 0.0;
- sumsq = 0.0;
- firstNonZero = TRUE;
- for (j = ilo; j <= ihi; j++) {
- jm1 = j + 1;
- if (ihi < j + 1) {
- jm1 = ihi;
- }
-
- for (i = ilo; i <= jm1; i++) {
- reAij = A[(i + 3 * (j - 1)) - 1].re;
- imAij = A[(i + 3 * (j - 1)) - 1].im;
- if (reAij != 0.0) {
- anorm = fabs(reAij);
- if (firstNonZero) {
- sumsq = 1.0;
- scale = anorm;
- firstNonZero = FALSE;
- } else if (scale < anorm) {
- temp2 = scale / anorm;
- sumsq = 1.0 + sumsq * temp2 * temp2;
- scale = anorm;
- } else {
- temp2 = anorm / scale;
- sumsq += temp2 * temp2;
- }
- }
-
- if (imAij != 0.0) {
- anorm = fabs(imAij);
- if (firstNonZero) {
- sumsq = 1.0;
- scale = anorm;
- firstNonZero = FALSE;
- } else if (scale < anorm) {
- temp2 = scale / anorm;
- sumsq = 1.0 + sumsq * temp2 * temp2;
- scale = anorm;
- } else {
- temp2 = anorm / scale;
- sumsq += temp2 * temp2;
- }
- }
- }
- }
-
- anorm = scale * sqrt(sumsq);
- }
-
- reAij = 2.2204460492503131E-16 * anorm;
- b_atol = 2.2250738585072014E-308;
- if (reAij > 2.2250738585072014E-308) {
- b_atol = reAij;
- }
-
- reAij = 2.2250738585072014E-308;
- if (anorm > 2.2250738585072014E-308) {
- reAij = anorm;
- }
-
- ascale = 1.0 / reAij;
- failed = TRUE;
- for (j = ihi; j + 1 < 4; j++) {
- alpha1[j] = A[j + 3 * j];
- }
-
- guard1 = FALSE;
- guard2 = FALSE;
- if (ihi >= ilo) {
- ifirst = ilo;
- istart = ilo;
- ilast = ihi - 1;
- ilastm1 = ihi - 2;
- iiter = 0;
- goto60 = FALSE;
- goto70 = FALSE;
- goto90 = FALSE;
- jiter = 1;
- do {
- exitg1 = 0;
- if (jiter <= 30 * ((ihi - ilo) + 1)) {
- if (ilast + 1 == ilo) {
- goto60 = TRUE;
- } else if (fabs(A[ilast + 3 * ilastm1].re) + fabs(A[ilast + 3 * ilastm1]
- .im) <= b_atol) {
- A[ilast + 3 * ilastm1].re = 0.0;
- A[ilast + 3 * ilastm1].im = 0.0;
- goto60 = TRUE;
- } else {
- j = ilastm1;
- exitg3 = FALSE;
- while ((exitg3 == FALSE) && (j + 1 >= ilo)) {
- if (j + 1 == ilo) {
- firstNonZero = TRUE;
- } else if (fabs(A[j + 3 * (j - 1)].re) + fabs(A[j + 3 * (j - 1)].im)
- <= b_atol) {
- A[j + 3 * (j - 1)].re = 0.0;
- A[j + 3 * (j - 1)].im = 0.0;
- firstNonZero = TRUE;
- } else {
- firstNonZero = FALSE;
- }
-
- if (firstNonZero) {
- ifirst = j + 1;
- goto70 = TRUE;
- exitg3 = TRUE;
- } else {
- j--;
- }
- }
- }
-
- if (goto60 || goto70) {
- firstNonZero = TRUE;
- } else {
- firstNonZero = FALSE;
- }
-
- if (!firstNonZero) {
- for (i = 0; i < 3; i++) {
- alpha1[i].re = rtNaN;
- alpha1[i].im = 0.0;
- beta1[i].re = rtNaN;
- beta1[i].im = 0.0;
- }
-
- for (jm1 = 0; jm1 < 3; jm1++) {
- for (jp1 = 0; jp1 < 3; jp1++) {
- Z[jp1 + 3 * jm1].re = rtNaN;
- Z[jp1 + 3 * jm1].im = 0.0;
- }
- }
-
- *info = -1.0;
- exitg1 = 1;
- } else {
- b_guard1 = FALSE;
- if (goto60) {
- goto60 = FALSE;
- alpha1[ilast] = A[ilast + 3 * ilast];
- ilast = ilastm1;
- ilastm1--;
- if (ilast + 1 < ilo) {
- failed = FALSE;
- guard2 = TRUE;
- exitg1 = 1;
- } else {
- iiter = 0;
- eshift_re = 0.0;
- eshift_im = 0.0;
- b_guard1 = TRUE;
- }
- } else {
- if (goto70) {
- goto70 = FALSE;
- iiter++;
- if (iiter - iiter / 10 * 10 != 0) {
- b_A.re = -(A[ilast + 3 * ilast].re - A[ilastm1 + 3 * ilastm1].re);
- b_A.im = -(A[ilast + 3 * ilast].im - A[ilastm1 + 3 * ilastm1].im);
- t1 = b_eml_div(b_A, 2.0);
- anorm = A[ilastm1 + 3 * ilast].re * A[ilast + 3 * ilastm1].re -
- A[ilastm1 + 3 * ilast].im * A[ilast + 3 * ilastm1].im;
- reAij = A[ilastm1 + 3 * ilast].re * A[ilast + 3 * ilastm1].im +
- A[ilastm1 + 3 * ilast].im * A[ilast + 3 * ilastm1].re;
- d.re = (t1.re * t1.re - t1.im * t1.im) + anorm;
- d.im = (t1.re * t1.im + t1.im * t1.re) + reAij;
- b_sqrt(&d);
- temp2 = A[ilastm1 + 3 * ilastm1].re - (t1.re - d.re);
- sigma1_im = A[ilastm1 + 3 * ilastm1].im - (t1.im - d.im);
- scale = A[ilastm1 + 3 * ilastm1].re - (t1.re + d.re);
- sumsq = A[ilastm1 + 3 * ilastm1].im - (t1.im + d.im);
- rho_re = temp2 - A[ilast + 3 * ilast].re;
- rho_im = sigma1_im - A[ilast + 3 * ilast].im;
- anorm = scale - A[ilast + 3 * ilast].re;
- reAij = sumsq - A[ilast + 3 * ilast].im;
- if (rt_hypotd_snf(fabs(rho_re), fabs(rho_im)) <= rt_hypotd_snf
- (fabs(anorm), fabs(reAij))) {
- scale = temp2;
- sumsq = sigma1_im;
- rho_re = t1.re - d.re;
- rho_im = t1.im - d.im;
- } else {
- rho_re = t1.re + d.re;
- rho_im = t1.im + d.im;
- }
- } else {
- eshift_re += A[ilast + 3 * ilastm1].re;
- eshift_im += A[ilast + 3 * ilastm1].im;
- scale = eshift_re;
- sumsq = eshift_im;
- }
-
- j = ilastm1;
- jp1 = ilastm1 + 1;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (j + 1 > ifirst)) {
- istart = j + 1;
- ctemp.re = A[j + 3 * j].re - scale;
- ctemp.im = A[j + 3 * j].im - sumsq;
- anorm = ascale * (fabs(ctemp.re) + fabs(ctemp.im));
- temp2 = ascale * (fabs(A[jp1 + 3 * j].re) + fabs(A[jp1 + 3 * j].
- im));
- reAij = anorm;
- if (temp2 > anorm) {
- reAij = temp2;
- }
-
- if ((reAij < 1.0) && (reAij != 0.0)) {
- anorm /= reAij;
- temp2 /= reAij;
- }
-
- if ((fabs(A[j + 3 * (j - 1)].re) + fabs(A[j + 3 * (j - 1)].im)) *
- temp2 <= anorm * b_atol) {
- goto90 = TRUE;
- exitg2 = TRUE;
- } else {
- jp1 = j;
- j--;
- }
- }
-
- if (!goto90) {
- istart = ifirst;
- if (ifirst == ilastm1 + 1) {
- ctemp.re = rho_re;
- ctemp.im = rho_im;
- } else {
- ctemp.re = A[(ifirst + 3 * (ifirst - 1)) - 1].re - scale;
- ctemp.im = A[(ifirst + 3 * (ifirst - 1)) - 1].im - sumsq;
- }
-
- goto90 = TRUE;
- }
- }
-
- if (goto90) {
- goto90 = FALSE;
- c_A = A[istart + 3 * (istart - 1)];
- b_eml_matlab_zlartg(ctemp, c_A, &imAij, &t1);
- j = istart;
- jm1 = istart - 2;
- while (j < ilast + 1) {
- if (j > istart) {
- c_A = A[1 + 3 * jm1];
- d_A = A[j + 3 * jm1];
- eml_matlab_zlartg(c_A, d_A, &imAij, &t1, &dc0);
- A[1 + 3 * jm1] = dc0;
- A[j + 3 * jm1].re = 0.0;
- A[j + 3 * jm1].im = 0.0;
- }
-
- for (jp1 = j - 1; jp1 + 1 < 4; jp1++) {
- d.re = imAij * A[(j + 3 * jp1) - 1].re;
- d.im = imAij * A[(j + 3 * jp1) - 1].im;
- temp2 = t1.re * A[j + 3 * jp1].re - t1.im * A[j + 3 * jp1].im;
- sigma1_im = t1.re * A[j + 3 * jp1].im + t1.im * A[j + 3 * jp1]
- .re;
- anorm = A[(j + 3 * jp1) - 1].re;
- reAij = A[(j + 3 * jp1) - 1].im;
- scale = A[(j + 3 * jp1) - 1].im;
- sumsq = A[(j + 3 * jp1) - 1].re;
- A[j + 3 * jp1].re = imAij * A[j + 3 * jp1].re - (t1.re * anorm
- + t1.im * reAij);
- A[j + 3 * jp1].im = imAij * A[j + 3 * jp1].im - (t1.re * scale
- - t1.im * sumsq);
- A[(j + 3 * jp1) - 1].re = d.re + temp2;
- A[(j + 3 * jp1) - 1].im = d.im + sigma1_im;
- }
-
- t1.re = -t1.re;
- t1.im = -t1.im;
- jp1 = j;
- if (ilast + 1 < j + 2) {
- jp1 = ilast - 1;
- }
-
- for (i = 0; i + 1 <= jp1 + 2; i++) {
- d.re = imAij * A[i + 3 * j].re;
- d.im = imAij * A[i + 3 * j].im;
- temp2 = t1.re * A[i + 3 * (j - 1)].re - t1.im * A[i + 3 * (j -
- 1)].im;
- sigma1_im = t1.re * A[i + 3 * (j - 1)].im + t1.im * A[i + 3 *
- (j - 1)].re;
- anorm = A[i + 3 * j].re;
- reAij = A[i + 3 * j].im;
- scale = A[i + 3 * j].im;
- sumsq = A[i + 3 * j].re;
- A[i + 3 * (j - 1)].re = imAij * A[i + 3 * (j - 1)].re - (t1.re
- * anorm + t1.im * reAij);
- A[i + 3 * (j - 1)].im = imAij * A[i + 3 * (j - 1)].im - (t1.re
- * scale - t1.im * sumsq);
- A[i + 3 * j].re = d.re + temp2;
- A[i + 3 * j].im = d.im + sigma1_im;
- }
-
- for (i = 0; i < 3; i++) {
- d.re = imAij * Z[i + 3 * j].re;
- d.im = imAij * Z[i + 3 * j].im;
- temp2 = t1.re * Z[i + 3 * (j - 1)].re - t1.im * Z[i + 3 * (j -
- 1)].im;
- sigma1_im = t1.re * Z[i + 3 * (j - 1)].im + t1.im * Z[i + 3 *
- (j - 1)].re;
- anorm = Z[i + 3 * j].re;
- reAij = Z[i + 3 * j].im;
- scale = Z[i + 3 * j].im;
- sumsq = Z[i + 3 * j].re;
- Z[i + 3 * (j - 1)].re = imAij * Z[i + 3 * (j - 1)].re - (t1.re
- * anorm + t1.im * reAij);
- Z[i + 3 * (j - 1)].im = imAij * Z[i + 3 * (j - 1)].im - (t1.re
- * scale - t1.im * sumsq);
- Z[i + 3 * j].re = d.re + temp2;
- Z[i + 3 * j].im = d.im + sigma1_im;
- }
-
- jm1 = j - 1;
- j++;
- }
- }
-
- b_guard1 = TRUE;
- }
-
- if (b_guard1 == TRUE) {
- jiter++;
- }
- }
- } else {
- guard2 = TRUE;
- exitg1 = 1;
- }
- } while (exitg1 == 0);
- } else {
- guard1 = TRUE;
- }
-
- if (guard2 == TRUE) {
- if (failed) {
- *info = (real_T)(ilast + 1);
- for (jp1 = 0; jp1 + 1 <= ilast + 1; jp1++) {
- alpha1[jp1].re = rtNaN;
- alpha1[jp1].im = 0.0;
- beta1[jp1].re = rtNaN;
- beta1[jp1].im = 0.0;
- }
-
- for (jm1 = 0; jm1 < 3; jm1++) {
- for (jp1 = 0; jp1 < 3; jp1++) {
- Z[jp1 + 3 * jm1].re = rtNaN;
- Z[jp1 + 3 * jm1].im = 0.0;
- }
- }
- } else {
- guard1 = TRUE;
- }
- }
-
- if (guard1 == TRUE) {
- for (j = 0; j + 1 < ilo; j++) {
- alpha1[j] = A[j + 3 * j];
- }
- }
-}
-
-static void eml_matlab_zlartg(const creal_T f, const creal_T g, real_T *cs,
- creal_T *sn, creal_T *r)
-{
- real_T scale;
- real_T f2s;
- real_T g2;
- real_T fs_re;
- real_T fs_im;
- real_T gs_re;
- real_T gs_im;
- int32_T count;
- int32_T rescaledir;
- boolean_T guard1 = FALSE;
- real_T g2s;
- scale = fabs(f.re);
- f2s = fabs(f.im);
- if (f2s > scale) {
- scale = f2s;
- }
-
- f2s = fabs(g.re);
- g2 = fabs(g.im);
- if (g2 > f2s) {
- f2s = g2;
- }
-
- if (f2s > scale) {
- scale = f2s;
- }
-
- fs_re = f.re;
- fs_im = f.im;
- gs_re = g.re;
- gs_im = g.im;
- count = 0;
- rescaledir = 0;
- guard1 = FALSE;
- if (scale >= 7.4428285367870146E+137) {
- do {
- count++;
- fs_re *= 1.3435752215134178E-138;
- fs_im *= 1.3435752215134178E-138;
- gs_re *= 1.3435752215134178E-138;
- gs_im *= 1.3435752215134178E-138;
- scale *= 1.3435752215134178E-138;
- } while (!(scale < 7.4428285367870146E+137));
-
- rescaledir = 1;
- guard1 = TRUE;
- } else if (scale <= 1.3435752215134178E-138) {
- if ((g.re == 0.0) && (g.im == 0.0)) {
- *cs = 1.0;
- sn->re = 0.0;
- sn->im = 0.0;
- *r = f;
- } else {
- do {
- count++;
- fs_re *= 7.4428285367870146E+137;
- fs_im *= 7.4428285367870146E+137;
- gs_re *= 7.4428285367870146E+137;
- gs_im *= 7.4428285367870146E+137;
- scale *= 7.4428285367870146E+137;
- } while (!(scale > 1.3435752215134178E-138));
-
- rescaledir = -1;
- guard1 = TRUE;
- }
- } else {
- guard1 = TRUE;
- }
-
- if (guard1 == TRUE) {
- scale = fs_re * fs_re + fs_im * fs_im;
- g2 = gs_re * gs_re + gs_im * gs_im;
- f2s = g2;
- if (1.0 > g2) {
- f2s = 1.0;
- }
-
- if (scale <= f2s * 2.0041683600089728E-292) {
- if ((f.re == 0.0) && (f.im == 0.0)) {
- *cs = 0.0;
- r->re = rt_hypotd_snf(fabs(g.re), fabs(g.im));
- r->im = 0.0;
- f2s = rt_hypotd_snf(fabs(gs_re), fabs(gs_im));
- sn->re = gs_re / f2s;
- sn->im = -gs_im / f2s;
- } else {
- g2s = sqrt(g2);
- *cs = rt_hypotd_snf(fabs(fs_re), fabs(fs_im)) / g2s;
- f2s = fabs(f.re);
- g2 = fabs(f.im);
- if (g2 > f2s) {
- f2s = g2;
- }
-
- if (f2s > 1.0) {
- f2s = rt_hypotd_snf(fabs(f.re), fabs(f.im));
- fs_re = f.re / f2s;
- fs_im = f.im / f2s;
- } else {
- g2 = 7.4428285367870146E+137 * f.re;
- scale = 7.4428285367870146E+137 * f.im;
- f2s = rt_hypotd_snf(fabs(g2), fabs(scale));
- fs_re = g2 / f2s;
- fs_im = scale / f2s;
- }
-
- gs_re /= g2s;
- gs_im = -gs_im / g2s;
- sn->re = fs_re * gs_re - fs_im * gs_im;
- sn->im = fs_re * gs_im + fs_im * gs_re;
- r->re = *cs * f.re + (sn->re * g.re - sn->im * g.im);
- r->im = *cs * f.im + (sn->re * g.im + sn->im * g.re);
- }
- } else {
- f2s = sqrt(1.0 + g2 / scale);
- r->re = f2s * fs_re;
- r->im = f2s * fs_im;
- *cs = 1.0 / f2s;
- f2s = scale + g2;
- g2 = r->re / f2s;
- f2s = r->im / f2s;
- sn->re = g2 * gs_re - f2s * -gs_im;
- sn->im = g2 * -gs_im + f2s * gs_re;
- if (rescaledir > 0) {
- for (rescaledir = 1; rescaledir <= count; rescaledir++) {
- r->re *= 7.4428285367870146E+137;
- r->im *= 7.4428285367870146E+137;
- }
- } else {
- if (rescaledir < 0) {
- for (rescaledir = 1; rescaledir <= count; rescaledir++) {
- r->re *= 1.3435752215134178E-138;
- r->im *= 1.3435752215134178E-138;
- }
- }
- }
- }
- }
-}
-
-static void eml_matlab_ztgevc(const creal_T A[9], creal_T V[9])
-{
- creal_T work1[3];
- creal_T work2[3];
- real_T rworka[3];
- int32_T i;
- real_T anorm;
- int32_T j;
- real_T y;
- real_T xmx;
- real_T ascale;
- int32_T je;
- real_T temp;
- real_T salpha_re;
- real_T salpha_im;
- real_T acoeff;
- boolean_T b0;
- boolean_T b1;
- real_T scale;
- real_T acoefa;
- int32_T jr;
- real_T dmin;
- creal_T d;
- creal_T b_work1;
- for (i = 0; i < 3; i++) {
- work1[i].re = 0.0;
- work1[i].im = 0.0;
- work2[i].re = 0.0;
- work2[i].im = 0.0;
- rworka[i] = 0.0;
- }
-
- anorm = fabs(A[0].re) + fabs(A[0].im);
- for (j = 0; j < 2; j++) {
- for (i = 0; i <= j; i++) {
- rworka[1 + j] += fabs(A[i + 3 * (1 + j)].re) + fabs(A[i + 3 * (1 + j)].im);
- }
-
- y = rworka[1 + j] + (fabs(A[(j + 3 * (1 + j)) + 1].re) + fabs(A[(j + 3 * (1
- + j)) + 1].im));
- if (y > anorm) {
- anorm = y;
- }
- }
-
- xmx = anorm;
- if (2.2250738585072014E-308 > anorm) {
- xmx = 2.2250738585072014E-308;
- }
-
- ascale = 1.0 / xmx;
- for (je = 0; je < 3; je++) {
- y = (fabs(A[(3 * (2 - je) - je) + 2].re) + fabs(A[(3 * (2 - je) - je) + 2].
- im)) * ascale;
- if (1.0 > y) {
- y = 1.0;
- }
-
- temp = 1.0 / y;
- salpha_re = ascale * (temp * A[(3 * (2 - je) - je) + 2].re);
- salpha_im = ascale * (temp * A[(3 * (2 - je) - je) + 2].im);
- acoeff = temp * ascale;
- if ((fabs(temp) >= 2.2250738585072014E-308) && (fabs(acoeff) <
- 3.0062525400134592E-292)) {
- b0 = TRUE;
- } else {
- b0 = FALSE;
- }
-
- if ((fabs(salpha_re) + fabs(salpha_im) >= 2.2250738585072014E-308) && (fabs
- (salpha_re) + fabs(salpha_im) < 3.0062525400134592E-292)) {
- b1 = TRUE;
- } else {
- b1 = FALSE;
- }
-
- scale = 1.0;
- if (b0) {
- xmx = anorm;
- if (3.3264005158911995E+291 < anorm) {
- xmx = 3.3264005158911995E+291;
- }
-
- scale = 3.0062525400134592E-292 / fabs(temp) * xmx;
- }
-
- if (b1) {
- xmx = 3.0062525400134592E-292 / (fabs(salpha_re) + fabs(salpha_im));
- if (xmx > scale) {
- scale = xmx;
- }
- }
-
- if (b0 || b1) {
- y = fabs(acoeff);
- xmx = fabs(salpha_re) + fabs(salpha_im);
- if (1.0 > y) {
- y = 1.0;
- }
-
- if (xmx > y) {
- y = xmx;
- }
-
- y = 1.0 / (2.2250738585072014E-308 * y);
- if (y < scale) {
- scale = y;
- }
-
- if (b0) {
- acoeff = ascale * (scale * temp);
- } else {
- acoeff *= scale;
- }
-
- if (b1) {
- salpha_re *= scale;
- salpha_im *= scale;
- } else {
- salpha_re *= scale;
- salpha_im *= scale;
- }
- }
-
- acoefa = fabs(acoeff);
- for (jr = 0; jr < 3; jr++) {
- work1[jr].re = 0.0;
- work1[jr].im = 0.0;
- }
-
- work1[2 - je].re = 1.0;
- work1[2 - je].im = 0.0;
- dmin = 2.2204460492503131E-16 * acoefa * anorm;
- y = 2.2204460492503131E-16 * (fabs(salpha_re) + fabs(salpha_im));
- if (y > dmin) {
- dmin = y;
- }
-
- if (2.2250738585072014E-308 > dmin) {
- dmin = 2.2250738585072014E-308;
- }
-
- for (jr = 0; jr <= 1 - je; jr++) {
- work1[jr].re = acoeff * A[jr + 3 * (2 - je)].re;
- work1[jr].im = acoeff * A[jr + 3 * (2 - je)].im;
- }
-
- work1[2 - je].re = 1.0;
- work1[2 - je].im = 0.0;
- for (j = 0; j <= 1 - je; j++) {
- i = (-je - j) + 1;
- d.re = acoeff * A[i + 3 * i].re - salpha_re;
- d.im = acoeff * A[i + 3 * i].im - salpha_im;
- if (fabs(d.re) + fabs(d.im) <= dmin) {
- d.re = dmin;
- d.im = 0.0;
- }
-
- if ((fabs(d.re) + fabs(d.im) < 1.0) && (fabs(work1[i].re) + fabs(work1[i].
- im) >= 1.4980776123852632E+307 * (fabs(d.re) + fabs(d.im)))) {
- temp = 1.0 / (fabs(work1[i].re) + fabs(work1[i].im));
- for (jr = 0; jr <= 2 - je; jr++) {
- work1[jr].re *= temp;
- work1[jr].im *= temp;
- }
- }
-
- b_work1.re = -work1[i].re;
- b_work1.im = -work1[i].im;
- work1[i] = c_eml_div(b_work1, d);
- if (i + 1 > 1) {
- if (fabs(work1[1].re) + fabs(work1[1].im) > 1.0) {
- temp = 1.0 / (fabs(work1[1].re) + fabs(work1[1].im));
- if (acoefa * rworka[1] >= 1.4980776123852632E+307 * temp) {
- for (jr = 0; jr <= 2 - je; jr++) {
- work1[jr].re *= temp;
- work1[jr].im *= temp;
- }
- }
- }
-
- xmx = acoeff * work1[1].re;
- scale = acoeff * work1[1].im;
- work1[0].re += xmx * A[3].re - scale * A[3].im;
- work1[0].im += xmx * A[3].im + scale * A[3].re;
- }
- }
-
- for (jr = 0; jr < 3; jr++) {
- work2[jr].re = 0.0;
- work2[jr].im = 0.0;
- }
-
- for (i = 0; i <= 2 - je; i++) {
- for (jr = 0; jr < 3; jr++) {
- xmx = V[jr + 3 * i].re * work1[i].re - V[jr + 3 * i].im * work1[i].im;
- scale = V[jr + 3 * i].re * work1[i].im + V[jr + 3 * i].im * work1[i].re;
- work2[jr].re += xmx;
- work2[jr].im += scale;
- }
- }
-
- xmx = fabs(work2[0].re) + fabs(work2[0].im);
- for (jr = 0; jr < 2; jr++) {
- y = fabs(work2[1 + jr].re) + fabs(work2[1 + jr].im);
- if (y > xmx) {
- xmx = y;
- }
- }
-
- if (xmx > 2.2250738585072014E-308) {
- temp = 1.0 / xmx;
- for (jr = 0; jr < 3; jr++) {
- V[jr + 3 * (2 - je)].re = temp * work2[jr].re;
- V[jr + 3 * (2 - je)].im = temp * work2[jr].im;
- }
- } else {
- for (jr = 0; jr < 3; jr++) {
- V[jr + 3 * (2 - je)].re = 0.0;
- V[jr + 3 * (2 - je)].im = 0.0;
- }
- }
- }
-}
-
-void b_eig(const real_T A[4], creal_T V[4], creal_T D[4])
-{
- creal_T b_A[4];
- int32_T j;
- creal_T beta1[2];
- creal_T alpha1[2];
- real_T colnorm;
- int32_T coltop;
- real_T scale;
- real_T absxk;
- real_T t;
- real_T alpha1_im;
- for (j = 0; j < 4; j++) {
- b_A[j].re = A[j];
- b_A[j].im = 0.0;
- }
-
- b_eml_matlab_zggev(b_A, &colnorm, alpha1, beta1, V);
- for (coltop = 0; coltop < 4; coltop += 2) {
- colnorm = 0.0;
- scale = 2.2250738585072014E-308;
- for (j = coltop; j + 1 <= coltop + 2; j++) {
- absxk = fabs(V[j].re);
- if (absxk > scale) {
- t = scale / absxk;
- colnorm = 1.0 + colnorm * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- colnorm += t * t;
- }
-
- absxk = fabs(V[j].im);
- if (absxk > scale) {
- t = scale / absxk;
- colnorm = 1.0 + colnorm * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- colnorm += t * t;
- }
- }
-
- colnorm = scale * sqrt(colnorm);
- for (j = coltop; j + 1 <= coltop + 2; j++) {
- V[j] = b_eml_div(V[j], colnorm);
- }
- }
-
- for (j = 0; j < 2; j++) {
- t = alpha1[j].re;
- alpha1_im = alpha1[j].im;
- if (beta1[j].im == 0.0) {
- if (alpha1[j].im == 0.0) {
- alpha1[j].re /= beta1[j].re;
- alpha1[j].im = 0.0;
- } else if (alpha1[j].re == 0.0) {
- alpha1[j].re = 0.0;
- alpha1[j].im = alpha1_im / beta1[j].re;
- } else {
- alpha1[j].re /= beta1[j].re;
- alpha1[j].im = alpha1_im / beta1[j].re;
- }
- } else if (beta1[j].re == 0.0) {
- if (alpha1[j].re == 0.0) {
- alpha1[j].re = alpha1[j].im / beta1[j].im;
- alpha1[j].im = 0.0;
- } else if (alpha1[j].im == 0.0) {
- alpha1[j].re = 0.0;
- alpha1[j].im = -(t / beta1[j].im);
- } else {
- alpha1[j].re = alpha1[j].im / beta1[j].im;
- alpha1[j].im = -(t / beta1[j].im);
- }
- } else {
- absxk = fabs(beta1[j].re);
- colnorm = fabs(beta1[j].im);
- if (absxk > colnorm) {
- colnorm = beta1[j].im / beta1[j].re;
- scale = beta1[j].re + colnorm * beta1[j].im;
- alpha1[j].re = (alpha1[j].re + colnorm * alpha1[j].im) / scale;
- alpha1[j].im = (alpha1_im - colnorm * t) / scale;
- } else if (colnorm == absxk) {
- if (beta1[j].re > 0.0) {
- colnorm = 0.5;
- } else {
- colnorm = -0.5;
- }
-
- if (beta1[j].im > 0.0) {
- scale = 0.5;
- } else {
- scale = -0.5;
- }
-
- alpha1[j].re = (alpha1[j].re * colnorm + alpha1[j].im * scale) / absxk;
- alpha1[j].im = (alpha1_im * colnorm - t * scale) / absxk;
- } else {
- colnorm = beta1[j].re / beta1[j].im;
- scale = beta1[j].im + colnorm * beta1[j].re;
- alpha1[j].re = (colnorm * alpha1[j].re + alpha1[j].im) / scale;
- alpha1[j].im = (colnorm * alpha1_im - t) / scale;
- }
- }
- }
-
- for (j = 0; j < 4; j++) {
- D[j].re = 0.0;
- D[j].im = 0.0;
- }
-
- for (j = 0; j < 2; j++) {
- D[j + (j << 1)] = alpha1[j];
- }
-}
-
-void eig(const real_T A[9], creal_T V[9], creal_T D[9])
-{
- creal_T b_A[9];
- int32_T j;
- creal_T beta1[3];
- creal_T alpha1[3];
- real_T colnorm;
- int32_T coltop;
- real_T scale;
- real_T absxk;
- real_T t;
- real_T alpha1_im;
- for (j = 0; j < 9; j++) {
- b_A[j].re = A[j];
- b_A[j].im = 0.0;
- }
-
- eml_matlab_zggev(b_A, &colnorm, alpha1, beta1, V);
- for (coltop = 0; coltop < 8; coltop += 3) {
- colnorm = 0.0;
- scale = 2.2250738585072014E-308;
- for (j = coltop; j + 1 <= coltop + 3; j++) {
- absxk = fabs(V[j].re);
- if (absxk > scale) {
- t = scale / absxk;
- colnorm = 1.0 + colnorm * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- colnorm += t * t;
- }
-
- absxk = fabs(V[j].im);
- if (absxk > scale) {
- t = scale / absxk;
- colnorm = 1.0 + colnorm * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- colnorm += t * t;
- }
- }
-
- colnorm = scale * sqrt(colnorm);
- for (j = coltop; j + 1 <= coltop + 3; j++) {
- V[j] = b_eml_div(V[j], colnorm);
- }
- }
-
- for (j = 0; j < 3; j++) {
- t = alpha1[j].re;
- alpha1_im = alpha1[j].im;
- if (beta1[j].im == 0.0) {
- if (alpha1[j].im == 0.0) {
- alpha1[j].re /= beta1[j].re;
- alpha1[j].im = 0.0;
- } else if (alpha1[j].re == 0.0) {
- alpha1[j].re = 0.0;
- alpha1[j].im = alpha1_im / beta1[j].re;
- } else {
- alpha1[j].re /= beta1[j].re;
- alpha1[j].im = alpha1_im / beta1[j].re;
- }
- } else if (beta1[j].re == 0.0) {
- if (alpha1[j].re == 0.0) {
- alpha1[j].re = alpha1[j].im / beta1[j].im;
- alpha1[j].im = 0.0;
- } else if (alpha1[j].im == 0.0) {
- alpha1[j].re = 0.0;
- alpha1[j].im = -(t / beta1[j].im);
- } else {
- alpha1[j].re = alpha1[j].im / beta1[j].im;
- alpha1[j].im = -(t / beta1[j].im);
- }
- } else {
- absxk = fabs(beta1[j].re);
- colnorm = fabs(beta1[j].im);
- if (absxk > colnorm) {
- colnorm = beta1[j].im / beta1[j].re;
- scale = beta1[j].re + colnorm * beta1[j].im;
- alpha1[j].re = (alpha1[j].re + colnorm * alpha1[j].im) / scale;
- alpha1[j].im = (alpha1_im - colnorm * t) / scale;
- } else if (colnorm == absxk) {
- if (beta1[j].re > 0.0) {
- colnorm = 0.5;
- } else {
- colnorm = -0.5;
- }
-
- if (beta1[j].im > 0.0) {
- scale = 0.5;
- } else {
- scale = -0.5;
- }
-
- alpha1[j].re = (alpha1[j].re * colnorm + alpha1[j].im * scale) / absxk;
- alpha1[j].im = (alpha1_im * colnorm - t * scale) / absxk;
- } else {
- colnorm = beta1[j].re / beta1[j].im;
- scale = beta1[j].im + colnorm * beta1[j].re;
- alpha1[j].re = (colnorm * alpha1[j].re + alpha1[j].im) / scale;
- alpha1[j].im = (colnorm * alpha1_im - t) / scale;
- }
- }
- }
-
- for (j = 0; j < 9; j++) {
- D[j].re = 0.0;
- D[j].im = 0.0;
- }
-
- for (j = 0; j < 3; j++) {
- D[j + 3 * j] = alpha1[j];
- }
-}
-
-/* End of code generation (eig.cpp) */
diff --git a/.svn/pristine/84/84998a2801e20ac9d6b7c248bdf23807e2136f48.svn-base b/.svn/pristine/84/84998a2801e20ac9d6b7c248bdf23807e2136f48.svn-base
deleted file mode 100644
index e52c384dd19681549b12d8d1121c88c746607513..0000000000000000000000000000000000000000
--- a/.svn/pristine/84/84998a2801e20ac9d6b7c248bdf23807e2136f48.svn-base
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_data.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_data'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-boolean_T method_not_empty;
-uint32_T state[625];
-
-/* Function Declarations */
-
-/* Function Definitions */
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_data.cpp) */
diff --git a/.svn/pristine/87/8775f48734abce50e024a25f7bf11ae2e2407daf.svn-base b/.svn/pristine/87/8775f48734abce50e024a25f7bf11ae2e2407daf.svn-base
deleted file mode 100644
index 35d40761fa519335fc71fb6a5f187e742aae0cda..0000000000000000000000000000000000000000
--- a/.svn/pristine/87/8775f48734abce50e024a25f7bf11ae2e2407daf.svn-base
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * rand.cpp
- *
- * Code generation for function 'rand'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "rand.h"
-#include "randn.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static void eml_rand_mt19937ar_stateful(real_T r[1843200]);
-
-/* Function Definitions */
-static void eml_rand_mt19937ar_stateful(real_T r[1843200])
-{
- int32_T k;
- real_T d0;
- for (k = 0; k < 1843200; k++) {
- d0 = genrandu(state);
- r[k] = d0;
- }
-}
-
-void b_rand(real_T r[1843200])
-{
- if (!method_not_empty) {
- method_not_empty = TRUE;
- }
-
- eml_rand_mt19937ar_stateful(r);
-}
-
-real_T c_rand(void)
-{
- if (!method_not_empty) {
- method_not_empty = TRUE;
- }
-
- return genrandu(state);
-}
-
-/* End of code generation (rand.cpp) */
diff --git a/.svn/pristine/8a/8a6b3eddc6e0d6fddd98062544c1d52a97f1040b.svn-base b/.svn/pristine/8a/8a6b3eddc6e0d6fddd98062544c1d52a97f1040b.svn-base
deleted file mode 100644
index 8a8f0f92a7a4229e009a60bc165314e37063886c..0000000000000000000000000000000000000000
--- a/.svn/pristine/8a/8a6b3eddc6e0d6fddd98062544c1d52a97f1040b.svn-base
+++ /dev/null
@@ -1,960 +0,0 @@
-/*
- * compute_prob1.cpp
- *
- * Code generation for function 'compute_prob1'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "compute_prob1.h"
-#include "norm.h"
-#include "estimateRigidTransform.h"
-#include "mean.h"
-#include "image_warping.h"
-#include "mldivide.h"
-#include "expm.h"
-#include "randn.h"
-#include "chol.h"
-#include "sum.h"
-#include "mrdivide.h"
-#include "diag.h"
-#include "log.h"
-#include "eig.h"
-#include "resampling.h"
-#include "exp.h"
-#include "repmat.h"
-#include "gradient.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static void b_eml_li_find(const boolean_T x[25], int32_T y_data[25], int32_T
- y_size[1]);
-static int32_T compute_nones(const boolean_T x[1296]);
-
-/* Function Definitions */
-static void b_eml_li_find(const boolean_T x[25], int32_T y_data[25], int32_T
- y_size[1])
-{
- int32_T k;
- int32_T i;
- k = 0;
- for (i = 0; i < 25; i++) {
- if (x[i]) {
- k++;
- }
- }
-
- y_size[0] = k;
- k = 0;
- for (i = 0; i < 25; i++) {
- if (x[i]) {
- y_data[k] = i + 1;
- k++;
- }
- }
-}
-
-static int32_T compute_nones(const boolean_T x[1296])
-{
- int32_T k;
- int32_T i;
- k = 0;
- for (i = 0; i < 1296; i++) {
- if (x[i]) {
- k++;
- }
- }
-
- return k;
-}
-
-void compute_prob1(real_T X_par[2250000], real_T X_par_pred[2250000], real_T
- AR_velocity[2250000], const real_T dw_dp[15552], real_T t,
- real_T center_x, real_T center_y, const real_T Ixyz[504063],
- const real_T point_matrix[3888], const real_T mean_img[3888],
- real_T tracked_images[38880000], real_T Aff_matrix[9], real_T
- centroid[3])
-{
- static real_T warped_img[3888];
- static real_T frame[504063];
- static real_T b_Ixyz[168021];
- static real_T c_Ixyz[168021];
- int32_T i21;
- int32_T i;
- static real_T ext_frame[1008126];
- static real_T ext_grad_x[336042];
- static real_T ext_grad_y[336042];
- real_T rigid_transform_par[400];
- real_T dist1Array[25];
- int32_T par;
- creal_T v[9];
- creal_T V[9];
- creal_T dcv0[3];
- creal_T b_V[9];
- int32_T ix;
- real_T LOGMX[9];
- real_T b_LOGMX[9];
- real_T X_par_temp[9];
- real_T mean_trans[2];
- static const int8_T iv9[3] = { 0, 0, 1 };
-
- boolean_T zero_ind_z[1296];
- boolean_T b_zero_ind_z;
- boolean_T c_zero_ind_z[1296];
- int32_T tmp_size[1];
- int32_T tmp_data[1296];
- int32_T b_tmp_size[1];
- int32_T b_tmp_data[1296];
- real_T warped_grad_x3[1296];
- int32_T warped_img_size[1];
- real_T d;
- int32_T b_warped_img_size[1];
- int32_T c_warped_img_size[1];
- real_T warped_grad_y3[1296];
- real_T img_grad[2592];
- real_T transform[16];
- static real_T b_warped_img[5184];
- static real_T b_transform[5184];
- static real_T a[7776];
- real_T b_X_par_temp[6];
- real_T c_X_par_temp[6];
- real_T d_X_par_temp[6];
- real_T e_X_par_temp[6];
- real_T dv13[6];
- real_T dv14[6];
- static real_T b_a[7776];
- real_T c_a[2592];
- real_T CH[36];
- real_T sum_jacobian[6];
- real_T sigma_12[6];
- static const real_T sigma_11[36] = { 0.00066666666666666675, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 2.6666666666666667E-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.002666666666666667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.6666666666666667E-5,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.2666666666666666, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 3.2666666666666666 };
-
- real_T y[6];
- real_T b;
- static const int8_T b_b[9] = { 0, 0, 0, 0, 0, 0, 0, 1, 0 };
-
- static const int8_T c_b[9] = { 0, 0, 0, 0, 0, 0, 1, 0, 0 };
-
- static const int8_T d_b[9] = { 0, 1, 0, 1, 0, 0, 0, 0, 0 };
-
- static const int8_T e_b[9] = { 0, 1, 0, -1, 0, 0, 0, 0, 0 };
-
- static const int8_T f_b[9] = { 1, 0, 0, 0, -1, 0, 0, 0, 0 };
-
- static const int8_T g_b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 0 };
-
- real_T b_sigma_12[6];
- real_T h_b[9];
- real_T b_center_x[2];
- int32_T d_warped_img_size[1];
- int32_T e_warped_img_size[1];
- int32_T f_warped_img_size[1];
- static real_T c_transform[5184];
- static real_T warped_img_tr[3888];
- static real_T b_warped_img_tr[3888];
- boolean_T b_dist1Array[25];
- int32_T c_tmp_data[25];
- real_T outindex[25];
- real_T b_X_par_pred[225];
- real_T b_rigid_transform_par[400];
- int32_T itmp;
- boolean_T exitg1;
- real_T mean_log[4];
- real_T c_X_par_pred[4];
- real_T b_X_par[4];
- real_T c_LOGMX[4];
- creal_T b_v[4];
- creal_T c_V[4];
- creal_T dcv1[2];
- creal_T d_V[4];
- real_T c_X_par[50];
- real_T c_center_x[2];
- int32_T g_warped_img_size[1];
- int32_T h_warped_img_size[1];
- int32_T i_warped_img_size[1];
- static real_T c_rigid_transform_par[5184];
- memset(&warped_img[0], 0, 3888U * sizeof(real_T));
- memset(&frame[0], 0, 504063U * sizeof(real_T));
- for (i21 = 0; i21 < 441; i21++) {
- for (i = 0; i < 381; i++) {
- frame[336042 + (i + 381 * i21)] = Ixyz[336042 + (i + 381 * i21)];
- b_Ixyz[i + 381 * i21] = (Ixyz[i + 381 * i21] + 2.0) * (real_T)(frame
- [336042 + (i + 381 * i21)] != 0.0);
- frame[i + 381 * i21] = b_Ixyz[i + 381 * i21];
- c_Ixyz[i + 381 * i21] = (Ixyz[168021 + (i + 381 * i21)] + 2.0) * (real_T)
- (frame[336042 + (i + 381 * i21)] != 0.0);
- frame[168021 + (i + 381 * i21)] = c_Ixyz[i + 381 * i21];
- }
- }
-
- repmat(frame, ext_frame);
- gradient(*(real_T (*)[168021])&frame[336042], b_Ixyz, c_Ixyz);
- b_repmat(c_Ixyz, ext_grad_x);
- b_repmat(b_Ixyz, ext_grad_y);
- for (par = 0; par < 25; par++) {
- eig(*(real_T (*)[9])&AR_velocity[9 * par + 225 * ((int32_T)(t - 1.0) - 1)],
- V, v);
- for (i21 = 0; i21 < 3; i21++) {
- dcv0[i21] = v[i21 << 2];
- }
-
- b_log(dcv0);
- diag(dcv0, v);
- for (i21 = 0; i21 < 3; i21++) {
- for (i = 0; i < 3; i++) {
- b_V[i21 + 3 * i].re = 0.0;
- b_V[i21 + 3 * i].im = 0.0;
- for (ix = 0; ix < 3; ix++) {
- b_V[i21 + 3 * i].re += V[i21 + 3 * ix].re * v[ix + 3 * i].re - V[i21 +
- 3 * ix].im * v[ix + 3 * i].im;
- b_V[i21 + 3 * i].im += V[i21 + 3 * ix].re * v[ix + 3 * i].im + V[i21 +
- 3 * ix].im * v[ix + 3 * i].re;
- }
- }
- }
-
- b_mrdivide(b_V, V, v);
- for (i21 = 0; i21 < 9; i21++) {
- LOGMX[i21] = v[i21].re * 0.5;
- }
-
- expm(LOGMX, b_LOGMX);
- for (i21 = 0; i21 < 3; i21++) {
- for (i = 0; i < 3; i++) {
- X_par_temp[i21 + 3 * i] = 0.0;
- for (ix = 0; ix < 3; ix++) {
- X_par_temp[i21 + 3 * i] += X_par[((i21 + 3 * ix) + 9 * par) + 225 *
- ((int32_T)(t - 1.0) - 1)] * b_LOGMX[ix + 3 * i];
- }
- }
- }
-
- mean_trans[0] = center_x;
- mean_trans[1] = center_y;
- for (i21 = 0; i21 < 2; i21++) {
- for (i = 0; i < 2; i++) {
- Aff_matrix[i + 3 * i21] = X_par_temp[i + 3 * i21];
- }
- }
-
- for (i21 = 0; i21 < 2; i21++) {
- Aff_matrix[6 + i21] = X_par_temp[6 + i21] + mean_trans[i21];
- }
-
- for (i21 = 0; i21 < 3; i21++) {
- Aff_matrix[2 + 3 * i21] = (real_T)iv9[i21];
- }
-
- b_image_warping(*(real_T (*)[336042])&ext_frame[0], Aff_matrix, point_matrix,
- *(real_T (*)[1296])&warped_img[0]);
- b_image_warping(*(real_T (*)[336042])&ext_frame[336042], Aff_matrix,
- point_matrix, *(real_T (*)[1296])&warped_img[1296]);
- b_image_warping(*(real_T (*)[336042])&ext_frame[672084], Aff_matrix,
- point_matrix, *(real_T (*)[1296])&warped_img[2592]);
-
- /* %% replace zeros with average values */
- for (i21 = 0; i21 < 1296; i21++) {
- b_zero_ind_z = (warped_img[2592 + i21] == 0.0);
- zero_ind_z[i21] = !b_zero_ind_z;
- c_zero_ind_z[i21] = b_zero_ind_z;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] + 2591];
- }
-
- d = mean(warped_grad_x3, warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] + 2591] = d;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- b_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] - 1];
- }
-
- d = mean(warped_grad_x3, b_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] - 1] = d;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- c_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] + 1295];
- }
-
- d = mean(warped_grad_x3, c_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] + 1295] = d;
- }
-
- /* %% */
- b_image_warping(ext_grad_x, Aff_matrix, point_matrix, warped_grad_x3);
- b_image_warping(ext_grad_y, Aff_matrix, point_matrix, warped_grad_y3);
- for (i21 = 0; i21 < 1296; i21++) {
- img_grad[i21] = warped_grad_x3[i21];
- img_grad[1296 + i21] = warped_grad_y3[i21];
- }
-
- /* %% compute rigid transform */
- estimateRigidTransform(mean_img, warped_img, transform);
- for (i21 = 0; i21 < 1296; i21++) {
- for (i = 0; i < 3; i++) {
- b_warped_img[i + (i21 << 2)] = warped_img[i21 + 1296 * i];
- }
-
- b_warped_img[3 + (i21 << 2)] = 1.0;
- for (i = 0; i < 4; i++) {
- b_transform[i21 + 1296 * i] = 0.0;
- for (ix = 0; ix < 4; ix++) {
- b_transform[i21 + 1296 * i] += transform[i + (ix << 2)] *
- b_warped_img[ix + (i21 << 2)];
- }
- }
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- warped_grad_x3[i21] = b_transform[2592 + i21] - mean_img[2592 + i21];
- }
-
- b_X_par_temp[0] = X_par_temp[0];
- b_X_par_temp[1] = X_par_temp[1];
- b_X_par_temp[2] = X_par_temp[3];
- b_X_par_temp[3] = X_par_temp[4];
- b_X_par_temp[4] = 0.0;
- b_X_par_temp[5] = 0.0;
- c_X_par_temp[0] = X_par_temp[0];
- c_X_par_temp[1] = X_par_temp[1];
- c_X_par_temp[2] = -X_par_temp[3];
- c_X_par_temp[3] = -X_par_temp[4];
- c_X_par_temp[4] = 0.0;
- c_X_par_temp[5] = 0.0;
- d_X_par_temp[0] = X_par_temp[3];
- d_X_par_temp[1] = X_par_temp[4];
- d_X_par_temp[2] = -X_par_temp[0];
- d_X_par_temp[3] = -X_par_temp[1];
- d_X_par_temp[4] = 0.0;
- d_X_par_temp[5] = 0.0;
- e_X_par_temp[0] = X_par_temp[3];
- e_X_par_temp[1] = X_par_temp[4];
- e_X_par_temp[2] = X_par_temp[0];
- e_X_par_temp[3] = X_par_temp[1];
- e_X_par_temp[4] = 0.0;
- e_X_par_temp[5] = 0.0;
- dv13[0] = 0.0;
- dv13[1] = 0.0;
- dv13[2] = 0.0;
- dv13[3] = 0.0;
- dv13[4] = X_par_temp[0];
- dv13[5] = X_par_temp[1];
- dv14[0] = 0.0;
- dv14[1] = 0.0;
- dv14[2] = 0.0;
- dv14[3] = 0.0;
- dv14[4] = X_par_temp[3];
- dv14[5] = X_par_temp[4];
- for (i = 0; i < 1296; i++) {
- d = 2.0 * warped_grad_x3[i];
- c_a[i] = d * img_grad[i];
- c_a[1296 + i] = d * img_grad[1296 + i];
- b_a[i] = c_a[i] * dw_dp[12 * i] + c_a[1296 + i] * dw_dp[1 + 12 * i];
- b_a[1296 + i] = c_a[i] * dw_dp[2 + 12 * i] + c_a[1296 + i] * dw_dp[3 + 12 *
- i];
- b_a[2592 + i] = c_a[i] * dw_dp[4 + 12 * i] + c_a[1296 + i] * dw_dp[5 + 12 *
- i];
- b_a[3888 + i] = c_a[i] * dw_dp[6 + 12 * i] + c_a[1296 + i] * dw_dp[7 + 12 *
- i];
- b_a[5184 + i] = c_a[i] * dw_dp[8 + 12 * i] + c_a[1296 + i] * dw_dp[9 + 12 *
- i];
- b_a[6480 + i] = c_a[i] * dw_dp[10 + 12 * i] + c_a[1296 + i] * dw_dp[11 +
- 12 * i];
- }
-
- for (i21 = 0; i21 < 6; i21++) {
- CH[i21] = b_X_par_temp[i21];
- CH[6 + i21] = c_X_par_temp[i21];
- CH[12 + i21] = d_X_par_temp[i21];
- CH[18 + i21] = e_X_par_temp[i21];
- CH[24 + i21] = dv13[i21];
- CH[30 + i21] = dv14[i21];
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- for (i = 0; i < 6; i++) {
- a[i21 + 1296 * i] = 0.0;
- for (ix = 0; ix < 6; ix++) {
- a[i21 + 1296 * i] += b_a[i21 + 1296 * ix] * CH[ix + 6 * i];
- }
- }
- }
-
- b_sum(a, sum_jacobian);
-
- /* eq 17 -> P */
- d = 0.0;
- for (i21 = 0; i21 < 6; i21++) {
- sigma_12[i21] = 0.0;
- for (i = 0; i < 6; i++) {
- sigma_12[i21] += sigma_11[i21 + 6 * i] * sum_jacobian[i];
- }
-
- /* eq 17 -> P*J' */
- y[i21] = 0.0;
- for (i = 0; i < 6; i++) {
- y[i21] += sum_jacobian[i] * sigma_11[i + 6 * i21];
- }
-
- d += y[i21] * sum_jacobian[i21];
- }
-
- /* eq 17 -> J*P*J'+R */
- b = -norm(warped_grad_x3);
- for (i = 0; i < 6; i++) {
- sum_jacobian[i] = sigma_12[i] / (d + 1.0) * b;
- }
-
- for (i21 = 0; i21 < 9; i21++) {
- LOGMX[i21] = ((((sum_jacobian[0] * (real_T)g_b[i21] + sum_jacobian[1] *
- (real_T)f_b[i21]) + sum_jacobian[2] * (real_T)e_b[i21]) +
- sum_jacobian[3] * (real_T)d_b[i21]) + sum_jacobian[4] *
- (real_T)c_b[i21]) + sum_jacobian[5] * (real_T)b_b[i21];
- }
-
- expm(LOGMX, b_LOGMX);
- for (i21 = 0; i21 < 6; i21++) {
- b_sigma_12[i21] = sigma_12[i21] / (d + 1.0);
- }
-
- for (i21 = 0; i21 < 6; i21++) {
- for (i = 0; i < 6; i++) {
- CH[i21 + 6 * i] = sigma_11[i21 + 6 * i] - b_sigma_12[i21] * sigma_12[i];
- }
- }
-
- chol(CH);
- randn(sigma_12);
- for (i21 = 0; i21 < 6; i21++) {
- sum_jacobian[i21] = 0.0;
- for (i = 0; i < 6; i++) {
- sum_jacobian[i21] += CH[i + 6 * i21] * sigma_12[i];
- }
- }
-
- for (i21 = 0; i21 < 9; i21++) {
- LOGMX[i21] = ((((sum_jacobian[0] * (real_T)g_b[i21] / 2.0 + sum_jacobian[1]
- * (real_T)f_b[i21]) + sum_jacobian[2] * (real_T)e_b[i21])
- + sum_jacobian[3] * (real_T)d_b[i21]) + sum_jacobian[4] *
- (real_T)c_b[i21]) + sum_jacobian[5] * (real_T)b_b[i21];
- }
-
- expm(LOGMX, h_b);
- for (i21 = 0; i21 < 3; i21++) {
- for (i = 0; i < 3; i++) {
- LOGMX[i21 + 3 * i] = 0.0;
- for (ix = 0; ix < 3; ix++) {
- LOGMX[i21 + 3 * i] += X_par_temp[i21 + 3 * ix] * b_LOGMX[ix + 3 * i];
- }
- }
- }
-
- for (i21 = 0; i21 < 3; i21++) {
- for (i = 0; i < 3; i++) {
- X_par_pred[((i21 + 3 * i) + 9 * par) + 225 * ((int32_T)t - 1)] = 0.0;
- for (ix = 0; ix < 3; ix++) {
- X_par_pred[((i21 + 3 * i) + 9 * par) + 225 * ((int32_T)t - 1)] +=
- LOGMX[i21 + 3 * ix] * h_b[ix + 3 * i];
- }
- }
- }
-
- mldivide(*(real_T (*)[9])&X_par[9 * par + 225 * ((int32_T)(t - 1.0) - 1)],
- *(real_T (*)[9])&X_par_pred[9 * par + 225 * ((int32_T)t - 1)],
- LOGMX);
- for (i21 = 0; i21 < 3; i21++) {
- for (i = 0; i < 3; i++) {
- AR_velocity[((i + 3 * i21) + 9 * par) + 225 * ((int32_T)t - 1)] =
- LOGMX[i + 3 * i21];
- }
- }
-
- b_center_x[0] = center_x;
- b_center_x[1] = center_y;
- for (i21 = 0; i21 < 2; i21++) {
- for (i = 0; i < 2; i++) {
- Aff_matrix[i + 3 * i21] = X_par_pred[((i + 3 * i21) + 9 * par) + 225 *
- ((int32_T)t - 1)];
- }
- }
-
- for (i21 = 0; i21 < 2; i21++) {
- Aff_matrix[6 + i21] = X_par_pred[6 + ((i21 + 9 * par) + 225 * ((int32_T)t
- - 1))] + b_center_x[i21];
- }
-
- for (i21 = 0; i21 < 3; i21++) {
- Aff_matrix[2 + 3 * i21] = (real_T)iv9[i21];
- }
-
- b_image_warping(*(real_T (*)[336042])&ext_frame[0], Aff_matrix, point_matrix,
- *(real_T (*)[1296])&warped_img[0]);
- b_image_warping(*(real_T (*)[336042])&ext_frame[336042], Aff_matrix,
- point_matrix, *(real_T (*)[1296])&warped_img[1296]);
- b_image_warping(*(real_T (*)[336042])&ext_frame[672084], Aff_matrix,
- point_matrix, *(real_T (*)[1296])&warped_img[2592]);
-
- /* %% replace zeros with average values */
- for (i21 = 0; i21 < 1296; i21++) {
- b_zero_ind_z = (warped_img[2592 + i21] == 0.0);
- zero_ind_z[i21] = !b_zero_ind_z;
- c_zero_ind_z[i21] = b_zero_ind_z;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- d_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] + 2591];
- }
-
- d = mean(warped_grad_x3, d_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] + 2591] = d;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- e_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] - 1];
- }
-
- d = mean(warped_grad_x3, e_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] - 1] = d;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- f_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] + 1295];
- }
-
- d = mean(warped_grad_x3, f_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] + 1295] = d;
- }
-
- /* %% compute rigid transform */
- estimateRigidTransform(mean_img, warped_img, transform);
- for (i21 = 0; i21 < 4; i21++) {
- for (i = 0; i < 4; i++) {
- rigid_transform_par[(i + (i21 << 2)) + (par << 4)] = transform[i + (i21 <<
- 2)];
- }
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- for (i = 0; i < 3; i++) {
- b_warped_img[i + (i21 << 2)] = warped_img[i21 + 1296 * i];
- }
-
- b_warped_img[3 + (i21 << 2)] = 1.0;
- for (i = 0; i < 4; i++) {
- c_transform[i21 + 1296 * i] = 0.0;
- for (ix = 0; ix < 4; ix++) {
- c_transform[i21 + 1296 * i] += transform[i + (ix << 2)] *
- b_warped_img[ix + (i21 << 2)];
- }
- }
- }
-
- for (i21 = 0; i21 < 3; i21++) {
- memcpy(&warped_img_tr[1296 * i21], &c_transform[1296 * i21], 1296U *
- sizeof(real_T));
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- b_warped_img_tr[i21] = warped_img_tr[2592 + i21] - mean_img[2592 + i21];
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- b_warped_img_tr[i21 + 1296] = warped_img_tr[1296 + i21] - mean_img[1296 +
- i21];
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- b_warped_img_tr[i21 + 2592] = warped_img_tr[i21] - mean_img[i21];
- }
-
- d = b_norm(b_warped_img_tr) / 3.0;
-
- /* %% */
- dist1Array[par] = d * d;
- }
-
- for (i = 0; i < 25; i++) {
- b_dist1Array[i] = (dist1Array[i] > 1000.0);
- }
-
- b_eml_li_find(b_dist1Array, c_tmp_data, tmp_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- dist1Array[c_tmp_data[i21] - 1] = 1000.0;
- }
-
- for (i21 = 0; i21 < 25; i21++) {
- dist1Array[i21] *= -0.5;
- }
-
- b_exp(dist1Array);
-
- /* % Particle resampling */
- d = c_sum(dist1Array);
- for (i21 = 0; i21 < 25; i21++) {
- dist1Array[i21] /= d;
- }
-
- resampling(dist1Array, outindex);
- for (i21 = 0; i21 < 25; i21++) {
- for (i = 0; i < 3; i++) {
- for (ix = 0; ix < 3; ix++) {
- b_X_par_pred[(ix + 3 * i) + 9 * i21] = X_par_pred[((ix + 3 * i) + 9 *
- ((int32_T)outindex[i21] - 1)) + 225 * ((int32_T)t - 1)];
- }
- }
- }
-
- for (i21 = 0; i21 < 25; i21++) {
- for (i = 0; i < 3; i++) {
- for (ix = 0; ix < 3; ix++) {
- X_par[((ix + 3 * i) + 9 * i21) + 225 * ((int32_T)t - 1)] = b_X_par_pred
- [(ix + 3 * i) + 9 * i21];
- }
- }
- }
-
- for (i21 = 0; i21 < 25; i21++) {
- for (i = 0; i < 3; i++) {
- for (ix = 0; ix < 3; ix++) {
- b_X_par_pred[(ix + 3 * i) + 9 * i21] = AR_velocity[((ix + 3 * i) + 9 *
- ((int32_T)outindex[i21] - 1)) + 225 * ((int32_T)t - 1)];
- }
- }
- }
-
- for (i21 = 0; i21 < 25; i21++) {
- for (i = 0; i < 3; i++) {
- for (ix = 0; ix < 3; ix++) {
- AR_velocity[((ix + 3 * i) + 9 * i21) + 225 * ((int32_T)t - 1)] =
- b_X_par_pred[(ix + 3 * i) + 9 * i21];
- }
- }
- }
-
- for (i21 = 0; i21 < 25; i21++) {
- for (i = 0; i < 4; i++) {
- for (ix = 0; ix < 4; ix++) {
- b_rigid_transform_par[(ix + (i << 2)) + (i21 << 4)] =
- rigid_transform_par[(ix + (i << 2)) + (((int32_T)outindex[i21] - 1) <<
- 4)];
- }
- }
- }
-
- for (i21 = 0; i21 < 25; i21++) {
- for (i = 0; i < 4; i++) {
- for (ix = 0; ix < 4; ix++) {
- rigid_transform_par[(ix + (i << 2)) + (i21 << 4)] =
- b_rigid_transform_par[(ix + (i << 2)) + (i21 << 4)];
- }
- }
- }
-
- /* % Computing affine state particle mean */
- i = 1;
- d = dist1Array[0];
- itmp = 0;
- if (rtIsNaN(dist1Array[0])) {
- ix = 1;
- exitg1 = FALSE;
- while ((exitg1 == FALSE) && (ix + 1 < 26)) {
- i = ix + 1;
- if (!rtIsNaN(dist1Array[ix])) {
- d = dist1Array[ix];
- itmp = ix;
- exitg1 = TRUE;
- } else {
- ix++;
- }
- }
- }
-
- if (i < 25) {
- while (i + 1 < 26) {
- if (dist1Array[i] > d) {
- d = dist1Array[i];
- itmp = i;
- }
-
- i++;
- }
- }
-
- for (i21 = 0; i21 < 4; i21++) {
- mean_log[i21] = 0.0;
- }
-
- for (par = 0; par < 25; par++) {
- for (i21 = 0; i21 < 2; i21++) {
- for (i = 0; i < 2; i++) {
- c_X_par_pred[i + (i21 << 1)] = X_par_pred[((i + 3 * i21) + 9 * itmp) +
- 225 * ((int32_T)t - 1)];
- }
- }
-
- for (i21 = 0; i21 < 2; i21++) {
- for (i = 0; i < 2; i++) {
- b_X_par[i + (i21 << 1)] = X_par[((i + 3 * i21) + 9 * par) + 225 *
- ((int32_T)t - 1)];
- }
- }
-
- b_mldivide(c_X_par_pred, b_X_par, c_LOGMX);
- b_eig(c_LOGMX, c_V, b_v);
- for (i21 = 0; i21 < 2; i21++) {
- dcv1[i21] = b_v[3 * i21];
- }
-
- c_log(dcv1);
- b_diag(dcv1, b_v);
- for (i21 = 0; i21 < 2; i21++) {
- for (i = 0; i < 2; i++) {
- d_V[i21 + (i << 1)].re = 0.0;
- d_V[i21 + (i << 1)].im = 0.0;
- for (ix = 0; ix < 2; ix++) {
- d_V[i21 + (i << 1)].re += c_V[i21 + (ix << 1)].re * b_v[ix + (i << 1)]
- .re - c_V[i21 + (ix << 1)].im * b_v[ix + (i << 1)].im;
- d_V[i21 + (i << 1)].im += c_V[i21 + (ix << 1)].re * b_v[ix + (i << 1)]
- .im + c_V[i21 + (ix << 1)].im * b_v[ix + (i << 1)].re;
- }
- }
- }
-
- c_mrdivide(d_V, c_V, b_v);
- for (i21 = 0; i21 < 4; i21++) {
- mean_log[i21] += b_v[i21].re / 25.0;
- }
- }
-
- b_expm(mean_log, c_LOGMX);
- for (i21 = 0; i21 < 25; i21++) {
- for (i = 0; i < 2; i++) {
- c_X_par[i + (i21 << 1)] = X_par[6 + ((i + 9 * i21) + 225 * ((int32_T)t - 1))];
- }
- }
-
- b_mean(c_X_par, mean_trans);
-
- /* mean_X_par(:,:,t) = [mean_gl mean_trans;0 0 1]; */
- /* % Tracked object image update */
- c_center_x[0] = center_x;
- c_center_x[1] = center_y;
- for (i21 = 0; i21 < 2; i21++) {
- for (i = 0; i < 2; i++) {
- c_X_par_pred[i21 + (i << 1)] = 0.0;
- for (ix = 0; ix < 2; ix++) {
- c_X_par_pred[i21 + (i << 1)] += X_par_pred[((i21 + 3 * ix) + 9 * itmp) +
- 225 * ((int32_T)t - 1)] * c_LOGMX[ix + (i << 1)];
- }
- }
- }
-
- for (i21 = 0; i21 < 2; i21++) {
- for (i = 0; i < 2; i++) {
- Aff_matrix[i + 3 * i21] = c_X_par_pred[i + (i21 << 1)];
- }
- }
-
- for (i21 = 0; i21 < 2; i21++) {
- Aff_matrix[6 + i21] = mean_trans[i21] + c_center_x[i21];
- }
-
- for (i21 = 0; i21 < 3; i21++) {
- Aff_matrix[2 + 3 * i21] = (real_T)iv9[i21];
- }
-
- /* %% apply rigid transform to the tracked image and replace zeros with average values */
- memset(&warped_img[0], 0, 3888U * sizeof(real_T));
- b_image_warping(*(real_T (*)[336042])&ext_frame[0], Aff_matrix, point_matrix, *
- (real_T (*)[1296])&warped_img[0]);
- b_image_warping(*(real_T (*)[336042])&ext_frame[336042], Aff_matrix,
- point_matrix, *(real_T (*)[1296])&warped_img[1296]);
- b_image_warping(*(real_T (*)[336042])&ext_frame[672084], Aff_matrix,
- point_matrix, *(real_T (*)[1296])&warped_img[2592]);
- for (i21 = 0; i21 < 1296; i21++) {
- b_zero_ind_z = (warped_img[2592 + i21] == 0.0);
- zero_ind_z[i21] = !b_zero_ind_z;
- c_zero_ind_z[i21] = b_zero_ind_z;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- g_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] + 2591];
- }
-
- d = mean(warped_grad_x3, g_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] + 2591] = d;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- h_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] - 1];
- }
-
- d = mean(warped_grad_x3, h_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] - 1] = d;
- }
-
- eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
- for (i = 0; i < 1296; i++) {
- zero_ind_z[i] = !c_zero_ind_z[i];
- }
-
- eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
- i_warped_img_size[0] = b_tmp_size[0];
- i = b_tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_grad_x3[i21] = warped_img[b_tmp_data[i21] + 1295];
- }
-
- d = mean(warped_grad_x3, i_warped_img_size);
- i = tmp_size[0];
- for (i21 = 0; i21 < i; i21++) {
- warped_img[tmp_data[i21] + 1295] = d;
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- for (i = 0; i < 3; i++) {
- b_warped_img[i + (i21 << 2)] = warped_img[i21 + 1296 * i];
- }
-
- b_warped_img[3 + (i21 << 2)] = 1.0;
- for (i = 0; i < 4; i++) {
- c_rigid_transform_par[i21 + 1296 * i] = 0.0;
- for (ix = 0; ix < 4; ix++) {
- c_rigid_transform_par[i21 + 1296 * i] += rigid_transform_par[(i + (ix <<
- 2)) + (itmp << 4)] * b_warped_img[ix + (i21 << 2)];
- }
- }
- }
-
- for (i21 = 0; i21 < 3; i21++) {
- memcpy(&warped_img_tr[1296 * i21], &c_rigid_transform_par[1296 * i21], 1296U
- * sizeof(real_T));
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- tracked_images[i21 + 1296 * ((int32_T)t - 1)] = warped_img_tr[i21];
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- tracked_images[12960000 + (i21 + 1296 * ((int32_T)t - 1))] = warped_img_tr
- [1296 + i21];
- }
-
- for (i21 = 0; i21 < 1296; i21++) {
- tracked_images[25920000 + (i21 + 1296 * ((int32_T)t - 1))] = warped_img_tr
- [2592 + i21];
- }
-
- c_mean(warped_img, centroid);
-
- /* if t >= init_size && rem(t,update_period) == 0 */
- /* mean_img(:,1) = mean(tracked_images(:,1:t,1),2); */
- /* mean_img(:,2) = mean(tracked_images(:,1:t,2),2); */
- /* mean_img(:,3) = mean(tracked_images(:,1:t,3),2); */
- /* %% replace non connected values with mean values in the mean img */
- /* mean_img = removeOutliers(mean_img); */
- /* end */
-}
-
-void eml_li_find(const boolean_T x[1296], int32_T y_data[1296], int32_T y_size[1])
-{
- int32_T j;
- int32_T i;
- y_size[0] = compute_nones(x);
- j = 0;
- for (i = 0; i < 1296; i++) {
- if (x[i]) {
- y_data[j] = i + 1;
- j++;
- }
- }
-}
-
-/* End of code generation (compute_prob1.cpp) */
diff --git a/.svn/pristine/8b/8b55802202be6f7b03d78f0f0067836eff7ac300.svn-base b/.svn/pristine/8b/8b55802202be6f7b03d78f0f0067836eff7ac300.svn-base
deleted file mode 100644
index b7d1fa26c44b10edf95f03cdf71189ae221bb3db..0000000000000000000000000000000000000000
--- a/.svn/pristine/8b/8b55802202be6f7b03d78f0f0067836eff7ac300.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * exp.h
- *
- * Code generation for function 'exp'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __EXP_H__
-#define __EXP_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_exp(real_T x[25]);
-#endif
-/* End of code generation (exp.h) */
diff --git a/.svn/pristine/8b/8bdfb6f0bd2e0bf7ddf417dc232443a7aa90cfd8.svn-base b/.svn/pristine/8b/8bdfb6f0bd2e0bf7ddf417dc232443a7aa90cfd8.svn-base
deleted file mode 100644
index ba64a2764bd4d8364cd7e51db31c63fc806226b3..0000000000000000000000000000000000000000
--- a/.svn/pristine/8b/8bdfb6f0bd2e0bf7ddf417dc232443a7aa90cfd8.svn-base
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * chol.cpp
- *
- * Code generation for function 'chol'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "chol.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void chol(real_T A[36])
-{
- int32_T info;
- int32_T colj;
- int32_T j;
- boolean_T exitg1;
- int32_T jj;
- real_T ajj;
- int32_T ix;
- int32_T iy;
- int32_T jmax;
- int32_T i24;
- real_T c;
- int32_T i;
- int32_T ia;
- info = 0;
- colj = 1;
- j = 1;
- exitg1 = FALSE;
- while ((exitg1 == FALSE) && (j < 7)) {
- jj = (colj + j) - 2;
- ajj = 0.0;
- if (j - 1 < 1) {
- } else {
- ix = colj;
- iy = colj;
- for (jmax = 1; jmax < j; jmax++) {
- ajj += A[ix - 1] * A[iy - 1];
- ix++;
- iy++;
- }
- }
-
- ajj = A[jj] - ajj;
- if (ajj > 0.0) {
- ajj = sqrt(ajj);
- A[jj] = ajj;
- if (j < 6) {
- if (j - 1 == 0) {
- } else {
- iy = jj + 6;
- i24 = (colj + 6 * (5 - j)) + 6;
- for (jmax = colj + 6; jmax <= i24; jmax += 6) {
- ix = colj;
- c = 0.0;
- i = (jmax + j) - 2;
- for (ia = jmax; ia <= i; ia++) {
- c += A[ia - 1] * A[ix - 1];
- ix++;
- }
-
- A[iy] += -c;
- iy += 6;
- }
- }
-
- ajj = 1.0 / ajj;
- i24 = jj + 6 * (5 - j);
- for (jmax = jj + 6; jmax + 1 <= i24 + 7; jmax += 6) {
- A[jmax] *= ajj;
- }
-
- colj += 6;
- }
-
- j++;
- } else {
- A[jj] = ajj;
- info = j;
- exitg1 = TRUE;
- }
- }
-
- if (info == 0) {
- jmax = 6;
- } else {
- jmax = info - 1;
- }
-
- for (j = 0; j + 1 <= jmax; j++) {
- for (i = j + 1; i + 1 <= jmax; i++) {
- A[i + 6 * j] = 0.0;
- }
- }
-}
-
-/* End of code generation (chol.cpp) */
diff --git a/.svn/pristine/8d/8d851f91cec3128411984dc1e9d6c81f8b9d41d8.svn-base b/.svn/pristine/8d/8d851f91cec3128411984dc1e9d6c81f8b9d41d8.svn-base
deleted file mode 100644
index f728ac403d1c25ba456188f00f601329c07fae13..0000000000000000000000000000000000000000
--- a/.svn/pristine/8d/8d851f91cec3128411984dc1e9d6c81f8b9d41d8.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * gradient.h
- *
- * Code generation for function 'gradient'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __GRADIENT_H__
-#define __GRADIENT_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void gradient(const real_T f[168021], real_T varargout_1[168021], real_T varargout_2[168021]);
-#endif
-/* End of code generation (gradient.h) */
diff --git a/.svn/pristine/90/90452773ff2b8ab5839cf5e6db6367603a1df374.svn-base b/.svn/pristine/90/90452773ff2b8ab5839cf5e6db6367603a1df374.svn-base
deleted file mode 100644
index ae42eabc28525b0a04ba5b69ad3fbd658c86f9fe..0000000000000000000000000000000000000000
--- a/.svn/pristine/90/90452773ff2b8ab5839cf5e6db6367603a1df374.svn-base
+++ /dev/null
@@ -1,681 +0,0 @@
-/*
- * svd.cpp
- *
- * Code generation for function 'svd'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "svd.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static void b_eml_xaxpy(int32_T n, real_T a, const real_T x[16], int32_T ix0,
- real_T y[4], int32_T iy0);
-static void b_eml_xscal(int32_T n, real_T a, real_T x[4], int32_T ix0);
-static void c_eml_xaxpy(int32_T n, real_T a, const real_T x[4], int32_T ix0,
- real_T y[16], int32_T iy0);
-static real_T c_eml_xnrm2(int32_T n, const real_T x[16], int32_T ix0);
-static real_T d_eml_xnrm2(int32_T n, const real_T x[4], int32_T ix0);
-static real_T eml_div(real_T x, real_T y);
-static void eml_xaxpy(int32_T n, real_T a, int32_T ix0, real_T y[16], int32_T
- iy0);
-static real_T eml_xdotc(int32_T n, const real_T x[16], int32_T ix0, const real_T
- y[16], int32_T iy0);
-static void eml_xgesvd(const real_T A[16], real_T U[16], real_T S[4], real_T V
- [16]);
-static void eml_xrot(real_T x[16], int32_T ix0, int32_T iy0, real_T c, real_T s);
-static void eml_xrotg(real_T *a, real_T *b, real_T *c, real_T *s);
-static void eml_xscal(int32_T n, real_T a, real_T x[16], int32_T ix0);
-static void eml_xswap(real_T x[16], int32_T ix0, int32_T iy0);
-
-/* Function Definitions */
-static void b_eml_xaxpy(int32_T n, real_T a, const real_T x[16], int32_T ix0,
- real_T y[4], int32_T iy0)
-{
- int32_T ix;
- int32_T iy;
- int32_T k;
- if (a == 0.0) {
- } else {
- ix = ix0 - 1;
- iy = iy0 - 1;
- for (k = 0; k < n; k++) {
- y[iy] += a * x[ix];
- ix++;
- iy++;
- }
- }
-}
-
-static void b_eml_xscal(int32_T n, real_T a, real_T x[4], int32_T ix0)
-{
- int32_T i23;
- int32_T k;
- i23 = (ix0 + n) - 1;
- for (k = ix0; k <= i23; k++) {
- x[k - 1] *= a;
- }
-}
-
-static void c_eml_xaxpy(int32_T n, real_T a, const real_T x[4], int32_T ix0,
- real_T y[16], int32_T iy0)
-{
- int32_T ix;
- int32_T iy;
- int32_T k;
- if (a == 0.0) {
- } else {
- ix = ix0 - 1;
- iy = iy0 - 1;
- for (k = 0; k < n; k++) {
- y[iy] += a * x[ix];
- ix++;
- iy++;
- }
- }
-}
-
-static real_T c_eml_xnrm2(int32_T n, const real_T x[16], int32_T ix0)
-{
- real_T y;
- real_T scale;
- int32_T kend;
- int32_T k;
- real_T absxk;
- real_T t;
- y = 0.0;
- scale = 2.2250738585072014E-308;
- kend = (ix0 + n) - 1;
- for (k = ix0; k <= kend; k++) {
- absxk = fabs(x[k - 1]);
- if (absxk > scale) {
- t = scale / absxk;
- y = 1.0 + y * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- y += t * t;
- }
- }
-
- return scale * sqrt(y);
-}
-
-static real_T d_eml_xnrm2(int32_T n, const real_T x[4], int32_T ix0)
-{
- real_T y;
- real_T scale;
- int32_T kend;
- int32_T k;
- real_T absxk;
- real_T t;
- y = 0.0;
- scale = 2.2250738585072014E-308;
- kend = (ix0 + n) - 1;
- for (k = ix0; k <= kend; k++) {
- absxk = fabs(x[k - 1]);
- if (absxk > scale) {
- t = scale / absxk;
- y = 1.0 + y * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- y += t * t;
- }
- }
-
- return scale * sqrt(y);
-}
-
-static real_T eml_div(real_T x, real_T y)
-{
- return x / y;
-}
-
-static void eml_xaxpy(int32_T n, real_T a, int32_T ix0, real_T y[16], int32_T
- iy0)
-{
- int32_T ix;
- int32_T iy;
- int32_T k;
- if ((n < 1) || (a == 0.0)) {
- } else {
- ix = ix0 - 1;
- iy = iy0 - 1;
- for (k = 0; k < n; k++) {
- y[iy] += a * y[ix];
- ix++;
- iy++;
- }
- }
-}
-
-static real_T eml_xdotc(int32_T n, const real_T x[16], int32_T ix0, const real_T
- y[16], int32_T iy0)
-{
- real_T d;
- int32_T ix;
- int32_T iy;
- int32_T k;
- d = 0.0;
- if (n < 1) {
- } else {
- ix = ix0;
- iy = iy0;
- for (k = 1; k <= n; k++) {
- d += x[ix - 1] * y[iy - 1];
- ix++;
- iy++;
- }
- }
-
- return d;
-}
-
-static void eml_xgesvd(const real_T A[16], real_T U[16], real_T S[4], real_T V
- [16])
-{
- real_T b_A[16];
- real_T s[4];
- real_T e[4];
- real_T work[4];
- int32_T i;
- int32_T q;
- int32_T qs;
- real_T ztest0;
- int32_T ii;
- int32_T m;
- real_T rt;
- real_T ztest;
- int32_T iter;
- real_T tiny;
- real_T snorm;
- int32_T exitg3;
- boolean_T exitg2;
- real_T sn;
- real_T varargin_1[5];
- boolean_T exitg1;
- real_T sqds;
- real_T b;
- memcpy(&b_A[0], (real_T *)&A[0], sizeof(real_T) << 4);
- for (i = 0; i < 4; i++) {
- s[i] = 0.0;
- e[i] = 0.0;
- work[i] = 0.0;
- }
-
- for (i = 0; i < 16; i++) {
- U[i] = 0.0;
- V[i] = 0.0;
- }
-
- for (q = 0; q < 3; q++) {
- qs = q + (q << 2);
- ztest0 = c_eml_xnrm2(4 - q, b_A, qs + 1);
- if (ztest0 > 0.0) {
- if (b_A[qs] < 0.0) {
- s[q] = -ztest0;
- } else {
- s[q] = ztest0;
- }
-
- eml_xscal(4 - q, eml_div(1.0, s[q]), b_A, qs + 1);
- b_A[qs]++;
- s[q] = -s[q];
- } else {
- s[q] = 0.0;
- }
-
- for (ii = q + 1; ii + 1 < 5; ii++) {
- i = q + (ii << 2);
- if (s[q] != 0.0) {
- ztest0 = -eml_div(eml_xdotc(4 - q, b_A, qs + 1, b_A, i + 1), b_A[q + (q <<
- 2)]);
- eml_xaxpy(4 - q, ztest0, qs + 1, b_A, i + 1);
- }
-
- e[ii] = b_A[i];
- }
-
- for (ii = q; ii + 1 < 5; ii++) {
- U[ii + (q << 2)] = b_A[ii + (q << 2)];
- }
-
- if (q + 1 <= 2) {
- ztest0 = d_eml_xnrm2(3 - q, e, q + 2);
- if (ztest0 == 0.0) {
- e[q] = 0.0;
- } else {
- if (e[q + 1] < 0.0) {
- e[q] = -ztest0;
- } else {
- e[q] = ztest0;
- }
-
- ztest0 = eml_div(1.0, e[q]);
- b_eml_xscal(3 - q, ztest0, e, q + 2);
- e[q + 1]++;
- }
-
- e[q] = -e[q];
- if (e[q] != 0.0) {
- for (ii = q + 1; ii + 1 < 5; ii++) {
- work[ii] = 0.0;
- }
-
- for (ii = q + 1; ii + 1 < 5; ii++) {
- b_eml_xaxpy(3 - q, e[ii], b_A, (q + (ii << 2)) + 2, work, q + 2);
- }
-
- for (ii = q + 1; ii + 1 < 5; ii++) {
- c_eml_xaxpy(3 - q, eml_div(-e[ii], e[q + 1]), work, q + 2, b_A, (q +
- (ii << 2)) + 2);
- }
- }
-
- for (ii = q + 1; ii + 1 < 5; ii++) {
- V[ii + (q << 2)] = e[ii];
- }
- }
- }
-
- m = 2;
- s[3] = b_A[15];
- e[2] = b_A[14];
- e[3] = 0.0;
- for (ii = 0; ii < 4; ii++) {
- U[12 + ii] = 0.0;
- }
-
- U[15] = 1.0;
- for (q = 2; q > -1; q += -1) {
- qs = q + (q << 2);
- if (s[q] != 0.0) {
- for (ii = q + 1; ii + 1 < 5; ii++) {
- i = (q + (ii << 2)) + 1;
- ztest0 = -eml_div(eml_xdotc(4 - q, U, qs + 1, U, i), U[qs]);
- eml_xaxpy(4 - q, ztest0, qs + 1, U, i);
- }
-
- for (ii = q; ii + 1 < 5; ii++) {
- U[ii + (q << 2)] = -U[ii + (q << 2)];
- }
-
- U[qs]++;
- for (ii = 1; ii <= q; ii++) {
- U[(ii + (q << 2)) - 1] = 0.0;
- }
- } else {
- for (ii = 0; ii < 4; ii++) {
- U[ii + (q << 2)] = 0.0;
- }
-
- U[qs] = 1.0;
- }
- }
-
- for (q = 3; q > -1; q += -1) {
- if ((q + 1 <= 2) && (e[q] != 0.0)) {
- i = (q + (q << 2)) + 2;
- for (ii = q + 1; ii + 1 < 5; ii++) {
- qs = (q + (ii << 2)) + 2;
- ztest0 = -eml_div(eml_xdotc(3 - q, V, i, V, qs), V[i - 1]);
- eml_xaxpy(3 - q, ztest0, i, V, qs);
- }
- }
-
- for (ii = 0; ii < 4; ii++) {
- V[ii + (q << 2)] = 0.0;
- }
-
- V[q + (q << 2)] = 1.0;
- }
-
- for (q = 0; q < 4; q++) {
- ztest0 = e[q];
- if (s[q] != 0.0) {
- rt = fabs(s[q]);
- ztest = eml_div(s[q], rt);
- s[q] = rt;
- if (q + 1 < 4) {
- ztest0 = eml_div(e[q], ztest);
- }
-
- eml_xscal(4, ztest, U, (q << 2) + 1);
- }
-
- if ((q + 1 < 4) && (ztest0 != 0.0)) {
- rt = fabs(ztest0);
- ztest = eml_div(rt, ztest0);
- ztest0 = rt;
- s[q + 1] *= ztest;
- eml_xscal(4, ztest, V, ((q + 1) << 2) + 1);
- }
-
- e[q] = ztest0;
- }
-
- iter = 0;
- tiny = eml_div(2.2250738585072014E-308, 2.2204460492503131E-16);
- snorm = 0.0;
- for (ii = 0; ii < 4; ii++) {
- ztest0 = fabs(s[ii]);
- ztest = fabs(e[ii]);
- if ((ztest0 >= ztest) || rtIsNaN(ztest)) {
- } else {
- ztest0 = ztest;
- }
-
- if ((snorm >= ztest0) || rtIsNaN(ztest0)) {
- } else {
- snorm = ztest0;
- }
- }
-
- while ((m + 2 > 0) && (!(iter >= 75))) {
- ii = m;
- do {
- exitg3 = 0;
- q = ii + 1;
- if (ii + 1 == 0) {
- exitg3 = 1;
- } else {
- ztest0 = fabs(e[ii]);
- if ((ztest0 <= 2.2204460492503131E-16 * (fabs(s[ii]) + fabs(s[ii + 1])))
- || (ztest0 <= tiny) || ((iter > 20) && (ztest0 <=
- 2.2204460492503131E-16 * snorm))) {
- e[ii] = 0.0;
- exitg3 = 1;
- } else {
- ii--;
- }
- }
- } while (exitg3 == 0);
-
- if (ii + 1 == m + 1) {
- i = 4;
- } else {
- qs = m + 2;
- i = m + 2;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (i >= ii + 1)) {
- qs = i;
- if (i == ii + 1) {
- exitg2 = TRUE;
- } else {
- ztest0 = 0.0;
- if (i < m + 2) {
- ztest0 = fabs(e[i - 1]);
- }
-
- if (i > ii + 2) {
- ztest0 += fabs(e[i - 2]);
- }
-
- ztest = fabs(s[i - 1]);
- if ((ztest <= 2.2204460492503131E-16 * ztest0) || (ztest <= tiny)) {
- s[i - 1] = 0.0;
- exitg2 = TRUE;
- } else {
- i--;
- }
- }
- }
-
- if (qs == ii + 1) {
- i = 3;
- } else if (qs == m + 2) {
- i = 1;
- } else {
- i = 2;
- q = qs;
- }
- }
-
- switch (i) {
- case 1:
- ztest = e[m];
- e[m] = 0.0;
- for (i = m; i + 1 >= q + 1; i--) {
- ztest0 = s[i];
- eml_xrotg(&ztest0, &ztest, &rt, &sn);
- s[i] = ztest0;
- if (i + 1 > q + 1) {
- ztest = -sn * e[i - 1];
- e[i - 1] *= rt;
- }
-
- eml_xrot(V, (i << 2) + 1, ((m + 1) << 2) + 1, rt, sn);
- }
- break;
-
- case 2:
- ztest = e[q - 1];
- e[q - 1] = 0.0;
- for (i = q; i + 1 <= m + 2; i++) {
- eml_xrotg(&s[i], &ztest, &rt, &sn);
- ztest = -sn * e[i];
- e[i] *= rt;
- eml_xrot(U, (i << 2) + 1, ((q - 1) << 2) + 1, rt, sn);
- }
- break;
-
- case 3:
- varargin_1[0] = fabs(s[m + 1]);
- varargin_1[1] = fabs(s[m]);
- varargin_1[2] = fabs(e[m]);
- varargin_1[3] = fabs(s[q]);
- varargin_1[4] = fabs(e[q]);
- i = 1;
- sn = varargin_1[0];
- if (rtIsNaN(varargin_1[0])) {
- qs = 2;
- exitg1 = FALSE;
- while ((exitg1 == FALSE) && (qs < 6)) {
- i = qs;
- if (!rtIsNaN(varargin_1[qs - 1])) {
- sn = varargin_1[qs - 1];
- exitg1 = TRUE;
- } else {
- qs++;
- }
- }
- }
-
- if (i < 5) {
- while (i + 1 < 6) {
- if (varargin_1[i] > sn) {
- sn = varargin_1[i];
- }
-
- i++;
- }
- }
-
- rt = eml_div(s[m + 1], sn);
- ztest0 = eml_div(s[m], sn);
- ztest = eml_div(e[m], sn);
- sqds = eml_div(s[q], sn);
- b = eml_div((ztest0 + rt) * (ztest0 - rt) + ztest * ztest, 2.0);
- ztest0 = rt * ztest;
- ztest0 *= ztest0;
- ztest = 0.0;
- if ((b != 0.0) || (ztest0 != 0.0)) {
- ztest = sqrt(b * b + ztest0);
- if (b < 0.0) {
- ztest = -ztest;
- }
-
- ztest = eml_div(ztest0, b + ztest);
- }
-
- ztest += (sqds + rt) * (sqds - rt);
- ztest0 = sqds * eml_div(e[q], sn);
- for (i = q + 1; i <= m + 1; i++) {
- eml_xrotg(&ztest, &ztest0, &rt, &sn);
- if (i > q + 1) {
- e[i - 2] = ztest;
- }
-
- ztest0 = rt * s[i - 1];
- ztest = sn * e[i - 1];
- e[i - 1] = rt * e[i - 1] - sn * s[i - 1];
- b = s[i];
- s[i] *= rt;
- eml_xrot(V, ((i - 1) << 2) + 1, (i << 2) + 1, rt, sn);
- s[i - 1] = ztest0 + ztest;
- ztest0 = sn * b;
- eml_xrotg(&s[i - 1], &ztest0, &rt, &sn);
- ztest = rt * e[i - 1] + sn * s[i];
- s[i] = -sn * e[i - 1] + rt * s[i];
- ztest0 = sn * e[i];
- e[i] *= rt;
- eml_xrot(U, ((i - 1) << 2) + 1, (i << 2) + 1, rt, sn);
- }
-
- e[m] = ztest;
- iter++;
- break;
-
- default:
- if (s[q] < 0.0) {
- s[q] = -s[q];
- eml_xscal(4, -1.0, V, (q << 2) + 1);
- }
-
- i = q + 1;
- while ((q + 1 < 4) && (s[q] < s[i])) {
- rt = s[q];
- s[q] = s[i];
- s[i] = rt;
- eml_xswap(V, (q << 2) + 1, ((q + 1) << 2) + 1);
- eml_xswap(U, (q << 2) + 1, ((q + 1) << 2) + 1);
- q = i;
- i++;
- }
-
- iter = 0;
- m--;
- break;
- }
- }
-
- for (i = 0; i < 4; i++) {
- S[i] = s[i];
- }
-}
-
-static void eml_xrot(real_T x[16], int32_T ix0, int32_T iy0, real_T c, real_T s)
-{
- int32_T ix;
- int32_T iy;
- int32_T k;
- real_T y;
- real_T b_y;
- ix = ix0 - 1;
- iy = iy0 - 1;
- for (k = 0; k < 4; k++) {
- y = c * x[ix];
- b_y = s * x[iy];
- x[iy] = c * x[iy] - s * x[ix];
- x[ix] = y + b_y;
- iy++;
- ix++;
- }
-}
-
-static void eml_xrotg(real_T *a, real_T *b, real_T *c, real_T *s)
-{
- real_T roe;
- real_T absa;
- real_T absb;
- real_T scale;
- real_T ads;
- real_T bds;
- roe = *b;
- absa = fabs(*a);
- absb = fabs(*b);
- if (absa > absb) {
- roe = *a;
- }
-
- scale = absa + absb;
- if (scale == 0.0) {
- *s = 0.0;
- *c = 1.0;
- ads = 0.0;
- scale = 0.0;
- } else {
- ads = absa / scale;
- bds = absb / scale;
- ads = scale * sqrt(ads * ads + bds * bds);
- if (roe < 0.0) {
- ads = -ads;
- }
-
- *c = *a / ads;
- *s = *b / ads;
- if (absa > absb) {
- scale = *s;
- } else if (*c != 0.0) {
- scale = 1.0 / *c;
- } else {
- scale = 1.0;
- }
- }
-
- *a = ads;
- *b = scale;
-}
-
-static void eml_xscal(int32_T n, real_T a, real_T x[16], int32_T ix0)
-{
- int32_T i22;
- int32_T k;
- i22 = (ix0 + n) - 1;
- for (k = ix0; k <= i22; k++) {
- x[k - 1] *= a;
- }
-}
-
-static void eml_xswap(real_T x[16], int32_T ix0, int32_T iy0)
-{
- int32_T ix;
- int32_T iy;
- int32_T k;
- real_T temp;
- ix = ix0 - 1;
- iy = iy0 - 1;
- for (k = 0; k < 4; k++) {
- temp = x[ix];
- x[ix] = x[iy];
- x[iy] = temp;
- ix++;
- iy++;
- }
-}
-
-void svd(const real_T A[16], real_T U[16], real_T S[16], real_T V[16])
-{
- real_T s[4];
- int32_T k;
- eml_xgesvd(A, U, s, V);
- memset(&S[0], 0, sizeof(real_T) << 4);
- for (k = 0; k < 4; k++) {
- S[k + (k << 2)] = s[k];
- }
-}
-
-/* End of code generation (svd.cpp) */
diff --git a/.svn/pristine/91/915f953b739d75abb7d4fab8d05db47f840e3d90.svn-base b/.svn/pristine/91/915f953b739d75abb7d4fab8d05db47f840e3d90.svn-base
deleted file mode 100644
index e4025b052d8fbd5bab544427875b651bcafabfb0..0000000000000000000000000000000000000000
--- a/.svn/pristine/91/915f953b739d75abb7d4fab8d05db47f840e3d90.svn-base
+++ /dev/null
@@ -1,718 +0,0 @@
-/*
- * expm.cpp
- *
- * Code generation for function 'expm'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "expm.h"
-#include "mldivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static void PadeApproximantOfDegree(const real_T A[9], real_T m, real_T F[9]);
-static void b_PadeApproximantOfDegree(const real_T A[4], real_T m, real_T F[4]);
-static real_T rt_powd_snf(real_T u0, real_T u1);
-
-/* Function Definitions */
-static void PadeApproximantOfDegree(const real_T A[9], real_T m, real_T F[9])
-{
- int32_T i8;
- int32_T k;
- real_T A2[9];
- int32_T i9;
- real_T U[9];
- real_T A4[9];
- real_T V[9];
- real_T d;
- real_T A3[9];
- real_T b_A4[9];
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- A2[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- A2[i8 + 3 * k] += A[i8 + 3 * i9] * A[i9 + 3 * k];
- }
- }
- }
-
- if (m == 3.0) {
- memcpy(&U[0], &A2[0], 9U * sizeof(real_T));
- for (k = 0; k < 3; k++) {
- U[k + 3 * k] += 60.0;
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- A4[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- A4[i8 + 3 * k] += A[i8 + 3 * i9] * U[i9 + 3 * k];
- }
- }
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- U[k + 3 * i8] = A4[k + 3 * i8];
- }
- }
-
- for (i8 = 0; i8 < 9; i8++) {
- V[i8] = 12.0 * A2[i8];
- }
-
- d = 120.0;
- } else {
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- A3[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- A3[i8 + 3 * k] += A2[i8 + 3 * i9] * A2[i9 + 3 * k];
- }
- }
- }
-
- if (m == 5.0) {
- for (i8 = 0; i8 < 9; i8++) {
- U[i8] = A3[i8] + 420.0 * A2[i8];
- }
-
- for (k = 0; k < 3; k++) {
- U[k + 3 * k] += 15120.0;
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- A4[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- A4[i8 + 3 * k] += A[i8 + 3 * i9] * U[i9 + 3 * k];
- }
- }
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- U[k + 3 * i8] = A4[k + 3 * i8];
- }
- }
-
- for (i8 = 0; i8 < 9; i8++) {
- V[i8] = 30.0 * A3[i8] + 3360.0 * A2[i8];
- }
-
- d = 30240.0;
- } else {
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- b_A4[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- b_A4[i8 + 3 * k] += A3[i8 + 3 * i9] * A2[i9 + 3 * k];
- }
- }
- }
-
- if (m == 7.0) {
- for (i8 = 0; i8 < 9; i8++) {
- U[i8] = (b_A4[i8] + 1512.0 * A3[i8]) + 277200.0 * A2[i8];
- }
-
- for (k = 0; k < 3; k++) {
- U[k + 3 * k] += 8.64864E+6;
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- A4[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- A4[i8 + 3 * k] += A[i8 + 3 * i9] * U[i9 + 3 * k];
- }
- }
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- U[k + 3 * i8] = A4[k + 3 * i8];
- }
- }
-
- for (i8 = 0; i8 < 9; i8++) {
- V[i8] = (56.0 * b_A4[i8] + 25200.0 * A3[i8]) + 1.99584E+6 * A2[i8];
- }
-
- d = 1.729728E+7;
- } else if (m == 9.0) {
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- V[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- V[i8 + 3 * k] += b_A4[i8 + 3 * i9] * A2[i9 + 3 * k];
- }
- }
- }
-
- for (i8 = 0; i8 < 9; i8++) {
- U[i8] = ((V[i8] + 3960.0 * b_A4[i8]) + 2.16216E+6 * A3[i8]) +
- 3.027024E+8 * A2[i8];
- }
-
- for (k = 0; k < 3; k++) {
- U[k + 3 * k] += 8.8216128E+9;
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- A4[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- A4[i8 + 3 * k] += A[i8 + 3 * i9] * U[i9 + 3 * k];
- }
- }
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- U[k + 3 * i8] = A4[k + 3 * i8];
- }
- }
-
- for (i8 = 0; i8 < 9; i8++) {
- V[i8] = ((90.0 * V[i8] + 110880.0 * b_A4[i8]) + 3.027024E+7 * A3[i8])
- + 2.0756736E+9 * A2[i8];
- }
-
- d = 1.76432256E+10;
- } else {
- for (i8 = 0; i8 < 9; i8++) {
- U[i8] = (3.352212864E+10 * b_A4[i8] + 1.05594705216E+13 * A3[i8]) +
- 1.1873537964288E+15 * A2[i8];
- }
-
- for (k = 0; k < 3; k++) {
- U[k + 3 * k] += 3.238237626624E+16;
- for (i8 = 0; i8 < 3; i8++) {
- A4[i8 + 3 * k] = (b_A4[i8 + 3 * k] + 16380.0 * A3[i8 + 3 * k]) +
- 4.08408E+7 * A2[i8 + 3 * k];
- }
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- d = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- d += b_A4[i8 + 3 * i9] * A4[i9 + 3 * k];
- }
-
- V[i8 + 3 * k] = d + U[i8 + 3 * k];
- }
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- U[i8 + 3 * k] = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- U[i8 + 3 * k] += A[i8 + 3 * i9] * V[i9 + 3 * k];
- }
-
- A4[k + 3 * i8] = (182.0 * b_A4[k + 3 * i8] + 960960.0 * A3[k + 3 *
- i8]) + 1.32324192E+9 * A2[k + 3 * i8];
- }
- }
-
- for (i8 = 0; i8 < 3; i8++) {
- for (k = 0; k < 3; k++) {
- d = 0.0;
- for (i9 = 0; i9 < 3; i9++) {
- d += b_A4[i8 + 3 * i9] * A4[i9 + 3 * k];
- }
-
- V[i8 + 3 * k] = ((d + 6.704425728E+11 * b_A4[i8 + 3 * k]) +
- 1.29060195264E+14 * A3[i8 + 3 * k]) +
- 7.7717703038976E+15 * A2[i8 + 3 * k];
- }
- }
-
- d = 6.476475253248E+16;
- }
- }
- }
-
- for (k = 0; k < 3; k++) {
- V[k + 3 * k] += d;
- }
-
- for (k = 0; k < 9; k++) {
- d = V[k] - U[k];
- V[k] += U[k];
- U[k] = d;
- }
-
- mldivide(U, V, F);
-}
-
-static void b_PadeApproximantOfDegree(const real_T A[4], real_T m, real_T F[4])
-{
- int32_T i19;
- int32_T k;
- real_T A2[4];
- int32_T i20;
- real_T U[4];
- real_T A4[4];
- real_T V[4];
- real_T d;
- real_T A3[4];
- real_T b_A4[4];
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- A2[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- A2[i19 + (k << 1)] += A[i19 + (i20 << 1)] * A[i20 + (k << 1)];
- }
- }
- }
-
- if (m == 3.0) {
- for (i19 = 0; i19 < 4; i19++) {
- U[i19] = A2[i19];
- }
-
- for (k = 0; k < 2; k++) {
- U[k + (k << 1)] += 60.0;
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- A4[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- A4[i19 + (k << 1)] += A[i19 + (i20 << 1)] * U[i20 + (k << 1)];
- }
- }
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- U[k + (i19 << 1)] = A4[k + (i19 << 1)];
- }
- }
-
- for (i19 = 0; i19 < 4; i19++) {
- V[i19] = 12.0 * A2[i19];
- }
-
- d = 120.0;
- } else {
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- A3[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- A3[i19 + (k << 1)] += A2[i19 + (i20 << 1)] * A2[i20 + (k << 1)];
- }
- }
- }
-
- if (m == 5.0) {
- for (i19 = 0; i19 < 4; i19++) {
- U[i19] = A3[i19] + 420.0 * A2[i19];
- }
-
- for (k = 0; k < 2; k++) {
- U[k + (k << 1)] += 15120.0;
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- A4[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- A4[i19 + (k << 1)] += A[i19 + (i20 << 1)] * U[i20 + (k << 1)];
- }
- }
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- U[k + (i19 << 1)] = A4[k + (i19 << 1)];
- }
- }
-
- for (i19 = 0; i19 < 4; i19++) {
- V[i19] = 30.0 * A3[i19] + 3360.0 * A2[i19];
- }
-
- d = 30240.0;
- } else {
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- b_A4[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- b_A4[i19 + (k << 1)] += A3[i19 + (i20 << 1)] * A2[i20 + (k << 1)];
- }
- }
- }
-
- if (m == 7.0) {
- for (i19 = 0; i19 < 4; i19++) {
- U[i19] = (b_A4[i19] + 1512.0 * A3[i19]) + 277200.0 * A2[i19];
- }
-
- for (k = 0; k < 2; k++) {
- U[k + (k << 1)] += 8.64864E+6;
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- A4[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- A4[i19 + (k << 1)] += A[i19 + (i20 << 1)] * U[i20 + (k << 1)];
- }
- }
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- U[k + (i19 << 1)] = A4[k + (i19 << 1)];
- }
- }
-
- for (i19 = 0; i19 < 4; i19++) {
- V[i19] = (56.0 * b_A4[i19] + 25200.0 * A3[i19]) + 1.99584E+6 * A2[i19];
- }
-
- d = 1.729728E+7;
- } else if (m == 9.0) {
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- V[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- V[i19 + (k << 1)] += b_A4[i19 + (i20 << 1)] * A2[i20 + (k << 1)];
- }
- }
- }
-
- for (i19 = 0; i19 < 4; i19++) {
- U[i19] = ((V[i19] + 3960.0 * b_A4[i19]) + 2.16216E+6 * A3[i19]) +
- 3.027024E+8 * A2[i19];
- }
-
- for (k = 0; k < 2; k++) {
- U[k + (k << 1)] += 8.8216128E+9;
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- A4[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- A4[i19 + (k << 1)] += A[i19 + (i20 << 1)] * U[i20 + (k << 1)];
- }
- }
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- U[k + (i19 << 1)] = A4[k + (i19 << 1)];
- }
- }
-
- for (i19 = 0; i19 < 4; i19++) {
- V[i19] = ((90.0 * V[i19] + 110880.0 * b_A4[i19]) + 3.027024E+7 *
- A3[i19]) + 2.0756736E+9 * A2[i19];
- }
-
- d = 1.76432256E+10;
- } else {
- for (i19 = 0; i19 < 4; i19++) {
- U[i19] = (3.352212864E+10 * b_A4[i19] + 1.05594705216E+13 * A3[i19]) +
- 1.1873537964288E+15 * A2[i19];
- }
-
- for (k = 0; k < 2; k++) {
- U[k + (k << 1)] += 3.238237626624E+16;
- for (i19 = 0; i19 < 2; i19++) {
- A4[i19 + (k << 1)] = (b_A4[i19 + (k << 1)] + 16380.0 * A3[i19 + (k <<
- 1)]) + 4.08408E+7 * A2[i19 + (k << 1)];
- }
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- d = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- d += b_A4[i19 + (i20 << 1)] * A4[i20 + (k << 1)];
- }
-
- V[i19 + (k << 1)] = d + U[i19 + (k << 1)];
- }
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- U[i19 + (k << 1)] = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- U[i19 + (k << 1)] += A[i19 + (i20 << 1)] * V[i20 + (k << 1)];
- }
-
- A4[k + (i19 << 1)] = (182.0 * b_A4[k + (i19 << 1)] + 960960.0 * A3[k
- + (i19 << 1)]) + 1.32324192E+9 * A2[k + (i19 <<
- 1)];
- }
- }
-
- for (i19 = 0; i19 < 2; i19++) {
- for (k = 0; k < 2; k++) {
- d = 0.0;
- for (i20 = 0; i20 < 2; i20++) {
- d += b_A4[i19 + (i20 << 1)] * A4[i20 + (k << 1)];
- }
-
- V[i19 + (k << 1)] = ((d + 6.704425728E+11 * b_A4[i19 + (k << 1)]) +
- 1.29060195264E+14 * A3[i19 + (k << 1)]) +
- 7.7717703038976E+15 * A2[i19 + (k << 1)];
- }
- }
-
- d = 6.476475253248E+16;
- }
- }
- }
-
- for (k = 0; k < 2; k++) {
- V[k + (k << 1)] += d;
- }
-
- for (k = 0; k < 4; k++) {
- d = V[k] - U[k];
- V[k] += U[k];
- U[k] = d;
- }
-
- b_mldivide(U, V, F);
-}
-
-static real_T rt_powd_snf(real_T u0, real_T u1)
-{
- real_T y;
- real_T d3;
- real_T d4;
- if (rtIsNaN(u0) || rtIsNaN(u1)) {
- y = rtNaN;
- } else {
- d3 = fabs(u0);
- d4 = fabs(u1);
- if (rtIsInf(u1)) {
- if (d3 == 1.0) {
- y = rtNaN;
- } else if (d3 > 1.0) {
- if (u1 > 0.0) {
- y = rtInf;
- } else {
- y = 0.0;
- }
- } else if (u1 > 0.0) {
- y = 0.0;
- } else {
- y = rtInf;
- }
- } else if (d4 == 0.0) {
- y = 1.0;
- } else if (d4 == 1.0) {
- if (u1 > 0.0) {
- y = u0;
- } else {
- y = 1.0 / u0;
- }
- } else if (u1 == 2.0) {
- y = u0 * u0;
- } else if ((u1 == 0.5) && (u0 >= 0.0)) {
- y = sqrt(u0);
- } else if ((u0 < 0.0) && (u1 > floor(u1))) {
- y = rtNaN;
- } else {
- y = pow(u0, u1);
- }
- }
-
- return y;
-}
-
-void b_expm(real_T A[4], real_T F[4])
-{
- real_T normA;
- int32_T j;
- boolean_T exitg2;
- real_T s;
- int32_T i;
- boolean_T exitg1;
- static const real_T theta[5] = { 0.01495585217958292, 0.253939833006323,
- 0.95041789961629319, 2.097847961257068, 5.3719203511481517 };
-
- static const int8_T iv8[5] = { 3, 5, 7, 9, 13 };
-
- int32_T eint;
- real_T b_F[4];
- int32_T i18;
- normA = 0.0;
- j = 0;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (j < 2)) {
- s = 0.0;
- for (i = 0; i < 2; i++) {
- s += fabs(A[i + (j << 1)]);
- }
-
- if (rtIsNaN(s)) {
- normA = rtNaN;
- exitg2 = TRUE;
- } else {
- if (s > normA) {
- normA = s;
- }
-
- j++;
- }
- }
-
- if (normA <= 5.3719203511481517) {
- i = 0;
- exitg1 = FALSE;
- while ((exitg1 == FALSE) && (i < 5)) {
- if (normA <= theta[i]) {
- b_PadeApproximantOfDegree(A, (real_T)iv8[i], F);
- exitg1 = TRUE;
- } else {
- i++;
- }
- }
- } else {
- normA /= 5.3719203511481517;
- if ((!rtIsInf(normA)) && (!rtIsNaN(normA))) {
- normA = frexp(normA, &eint);
- } else {
- eint = 0;
- }
-
- s = (real_T)eint;
- if (normA == 0.5) {
- s = (real_T)eint - 1.0;
- }
-
- normA = rt_powd_snf(2.0, s);
- for (i = 0; i < 4; i++) {
- A[i] /= normA;
- }
-
- b_PadeApproximantOfDegree(A, 13.0, F);
- for (j = 0; j < (int32_T)s; j++) {
- for (i = 0; i < 2; i++) {
- for (eint = 0; eint < 2; eint++) {
- b_F[i + (eint << 1)] = 0.0;
- for (i18 = 0; i18 < 2; i18++) {
- b_F[i + (eint << 1)] += F[i + (i18 << 1)] * F[i18 + (eint << 1)];
- }
- }
- }
-
- for (i = 0; i < 2; i++) {
- for (eint = 0; eint < 2; eint++) {
- F[eint + (i << 1)] = b_F[eint + (i << 1)];
- }
- }
- }
- }
-}
-
-void expm(real_T A[9], real_T F[9])
-{
- real_T normA;
- int32_T j;
- boolean_T exitg2;
- real_T s;
- int32_T i;
- boolean_T exitg1;
- static const real_T theta[5] = { 0.01495585217958292, 0.253939833006323,
- 0.95041789961629319, 2.097847961257068, 5.3719203511481517 };
-
- static const int8_T iv5[5] = { 3, 5, 7, 9, 13 };
-
- int32_T eint;
- real_T b_F[9];
- int32_T i7;
- normA = 0.0;
- j = 0;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (j < 3)) {
- s = 0.0;
- for (i = 0; i < 3; i++) {
- s += fabs(A[i + 3 * j]);
- }
-
- if (rtIsNaN(s)) {
- normA = rtNaN;
- exitg2 = TRUE;
- } else {
- if (s > normA) {
- normA = s;
- }
-
- j++;
- }
- }
-
- if (normA <= 5.3719203511481517) {
- i = 0;
- exitg1 = FALSE;
- while ((exitg1 == FALSE) && (i < 5)) {
- if (normA <= theta[i]) {
- PadeApproximantOfDegree(A, (real_T)iv5[i], F);
- exitg1 = TRUE;
- } else {
- i++;
- }
- }
- } else {
- normA /= 5.3719203511481517;
- if ((!rtIsInf(normA)) && (!rtIsNaN(normA))) {
- normA = frexp(normA, &eint);
- } else {
- eint = 0;
- }
-
- s = (real_T)eint;
- if (normA == 0.5) {
- s = (real_T)eint - 1.0;
- }
-
- normA = rt_powd_snf(2.0, s);
- for (i = 0; i < 9; i++) {
- A[i] /= normA;
- }
-
- PadeApproximantOfDegree(A, 13.0, F);
- for (j = 0; j < (int32_T)s; j++) {
- for (i = 0; i < 3; i++) {
- for (eint = 0; eint < 3; eint++) {
- b_F[i + 3 * eint] = 0.0;
- for (i7 = 0; i7 < 3; i7++) {
- b_F[i + 3 * eint] += F[i + 3 * i7] * F[i7 + 3 * eint];
- }
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (eint = 0; eint < 3; eint++) {
- F[eint + 3 * i] = b_F[eint + 3 * i];
- }
- }
- }
- }
-}
-
-/* End of code generation (expm.cpp) */
diff --git a/.svn/pristine/94/942c2dc2083c346ff1a8a4589135a2e2283f15e3.svn-base b/.svn/pristine/94/942c2dc2083c346ff1a8a4589135a2e2283f15e3.svn-base
deleted file mode 100644
index aadd2b35dfed574448e5a362a37c1b544a66bccd..0000000000000000000000000000000000000000
--- a/.svn/pristine/94/942c2dc2083c346ff1a8a4589135a2e2283f15e3.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * exp.cpp
- *
- * Code generation for function 'exp'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "exp.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_exp(real_T x[25])
-{
- int32_T k;
- for (k = 0; k < 25; k++) {
- x[k] = exp(x[k]);
- }
-}
-
-/* End of code generation (exp.cpp) */
diff --git a/.svn/pristine/95/951ef356471bd1fd5f78b64e9f9f0b8070b9bbb8.svn-base b/.svn/pristine/95/951ef356471bd1fd5f78b64e9f9f0b8070b9bbb8.svn-base
deleted file mode 100644
index 08ebc1d3157c0b37c70ff435919244163d2fb833..0000000000000000000000000000000000000000
--- a/.svn/pristine/95/951ef356471bd1fd5f78b64e9f9f0b8070b9bbb8.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * repmat.h
- *
- * Code generation for function 'repmat'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __REPMAT_H__
-#define __REPMAT_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_repmat(const real_T a[168021], real_T b[336042]);
-extern void repmat(const real_T a[504063], real_T b[1008126]);
-#endif
-/* End of code generation (repmat.h) */
diff --git a/.svn/pristine/95/952008ecc8b7d9ae75a440609fccbeee53d8feaa.svn-base b/.svn/pristine/95/952008ecc8b7d9ae75a440609fccbeee53d8feaa.svn-base
deleted file mode 100644
index 31c60f7eaa6acda2d3ddd579a0874fbce349ad93..0000000000000000000000000000000000000000
--- a/.svn/pristine/95/952008ecc8b7d9ae75a440609fccbeee53d8feaa.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * rand.h
- *
- * Code generation for function 'rand'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RAND_H__
-#define __RAND_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_rand(real_T r[1843200]);
-extern real_T c_rand(void);
-#endif
-/* End of code generation (rand.h) */
diff --git a/.svn/pristine/97/97b06c91918fa9f39a1c11b6a26a09a02d168b6b.svn-base b/.svn/pristine/97/97b06c91918fa9f39a1c11b6a26a09a02d168b6b.svn-base
deleted file mode 100644
index 8766e8f5711b4fb50ae6f4a3c607103cc36e5bc1..0000000000000000000000000000000000000000
--- a/.svn/pristine/97/97b06c91918fa9f39a1c11b6a26a09a02d168b6b.svn-base
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_initialize.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_initialize'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_initialize.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void Optimal_affine_tracking_3d16_fast_realtime_initialize(void)
-{
- uint32_T r;
- int32_T mti;
- rt_InitInfAndNaN(8U);
- method_not_empty = FALSE;
- memset(&state[0], 0, 625U * sizeof(uint32_T));
- r = 5489U;
- state[0] = 5489U;
- for (mti = 0; mti < 623; mti++) {
- r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
- state[1 + mti] = r;
- }
-
- memset(&state[0], 0, 625U * sizeof(uint32_T));
- r = 5489U;
- state[0] = 5489U;
- for (mti = 0; mti < 623; mti++) {
- r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
- state[1 + mti] = r;
- }
-
- memset(&state[0], 0, 625U * sizeof(uint32_T));
- r = 5489U;
- state[0] = 5489U;
- for (mti = 0; mti < 623; mti++) {
- r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
- state[1 + mti] = r;
- }
-
- state[624] = 624U;
-}
-
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_initialize.cpp) */
diff --git a/.svn/pristine/98/98ea786b57e2c3a13a445a7cabe6b8c2db72fe14.svn-base b/.svn/pristine/98/98ea786b57e2c3a13a445a7cabe6b8c2db72fe14.svn-base
deleted file mode 100644
index 9d047f0fff79cb7128292ccfa0a84ae9b13fc544..0000000000000000000000000000000000000000
--- a/.svn/pristine/98/98ea786b57e2c3a13a445a7cabe6b8c2db72fe14.svn-base
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_terminate.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_terminate'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void Optimal_affine_tracking_3d16_fast_realtime_terminate(void)
-{
- /* (no terminate code required) */
-}
-
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_terminate.cpp) */
diff --git a/.svn/pristine/9a/9a2781f5f78cae6906c316fddd7d9784d5d82085.svn-base b/.svn/pristine/9a/9a2781f5f78cae6906c316fddd7d9784d5d82085.svn-base
deleted file mode 100644
index 7df5b72e1d675ecde17684b6831831dae6c42b03..0000000000000000000000000000000000000000
--- a/.svn/pristine/9a/9a2781f5f78cae6906c316fddd7d9784d5d82085.svn-base
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * repmat.cpp
- *
- * Code generation for function 'repmat'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "repmat.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static int32_T div_s32(int32_T numerator, int32_T denominator);
-
-/* Function Definitions */
-static int32_T div_s32(int32_T numerator, int32_T denominator)
-{
- int32_T quotient;
- uint32_T absNumerator;
- uint32_T absDenominator;
- int32_T quotientNeedsNegation;
- if (denominator == 0) {
- if (numerator >= 0) {
- quotient = MAX_int32_T;
- } else {
- quotient = MIN_int32_T;
- }
- } else {
- if (numerator >= 0) {
- absNumerator = (uint32_T)numerator;
- } else {
- absNumerator = (uint32_T)-numerator;
- }
-
- if (denominator >= 0) {
- absDenominator = (uint32_T)denominator;
- } else {
- absDenominator = (uint32_T)-denominator;
- }
-
- quotientNeedsNegation = (int32_T)((int32_T)(numerator < 0) != (int32_T)
- (denominator < 0));
- absNumerator /= absDenominator;
- if ((uint32_T)quotientNeedsNegation) {
- quotient = -(int32_T)absNumerator;
- } else {
- quotient = (int32_T)absNumerator;
- }
- }
-
- return quotient;
-}
-
-void b_repmat(const real_T a[168021], real_T b[336042])
-{
- int32_T ib;
- int32_T jtilecol;
- int32_T iacol;
- int32_T jcol;
- int32_T k;
- ib = 0;
- for (jtilecol = 0; jtilecol < 2; jtilecol++) {
- iacol = 1;
- for (jcol = 0; jcol < 441; jcol++) {
- for (k = 0; k < 381; k++) {
- b[ib] = a[iacol - 1];
- iacol++;
- ib++;
- }
- }
- }
-}
-
-void repmat(const real_T a[504063], real_T b[1008126])
-{
- int32_T db[3];
- int32_T da[3];
- int32_T ibtmp;
- int32_T k;
- static const int16_T iv2[3] = { 381, 441, 3 };
-
- static const int16_T iv3[3] = { 381, 882, 3 };
-
- int32_T ib;
- int32_T ia;
- int32_T r;
- int32_T u1;
- for (ibtmp = 0; ibtmp < 3; ibtmp++) {
- db[ibtmp] = 1;
- da[ibtmp] = 1;
- }
-
- for (k = 0; k < 2; k++) {
- da[k + 1] = da[k] * iv2[k];
- db[k + 1] = db[k] * iv3[k];
- }
-
- for (ib = 0; ib < 1008126; ib++) {
- ia = 0;
- ibtmp = ib;
- for (k = 2; k > -1; k += -1) {
- r = ibtmp - div_s32(ibtmp, db[k]) * db[k];
- ibtmp = div_s32(ibtmp - r, db[k]);
- u1 = iv2[k];
- ibtmp -= u1 * div_s32(ibtmp, u1);
- ia += da[k] * ibtmp;
- ibtmp = r;
- }
-
- b[ib] = a[ia];
- }
-}
-
-/* End of code generation (repmat.cpp) */
diff --git a/.svn/pristine/9a/9af9bb2680b599038296d79e79705e5226cd2ff2.svn-base b/.svn/pristine/9a/9af9bb2680b599038296d79e79705e5226cd2ff2.svn-base
deleted file mode 100644
index df3cc91a28eba957aa689275a56b3b857e3ebdf8..0000000000000000000000000000000000000000
--- a/.svn/pristine/9a/9af9bb2680b599038296d79e79705e5226cd2ff2.svn-base
+++ /dev/null
@@ -1,175 +0,0 @@
-/*
- * rtwtypes.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RTWTYPES_H__
-#define __RTWTYPES_H__
-#ifndef TRUE
-# define TRUE (1U)
-#endif
-#ifndef FALSE
-# define FALSE (0U)
-#endif
-#ifndef __TMWTYPES__
-#define __TMWTYPES__
-
-#include <limits.h>
-
-/*=======================================================================*
- * Target hardware information
- * Device type: Generic->MATLAB Host Computer
- * Number of bits: char: 8 short: 16 int: 32
- * long: 64 native word size: 64
- * Byte ordering: LittleEndian
- * Signed integer division rounds to: Zero
- * Shift right on a signed integer as arithmetic shift: on
- *=======================================================================*/
-
-/*=======================================================================*
- * Fixed width word size data types: *
- * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
- * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
- * real32_T, real64_T - 32 and 64 bit floating point numbers *
- *=======================================================================*/
-
-typedef signed char int8_T;
-typedef unsigned char uint8_T;
-typedef short int16_T;
-typedef unsigned short uint16_T;
-typedef int int32_T;
-typedef unsigned int uint32_T;
-typedef long int64_T;
-typedef unsigned long uint64_T;
-typedef float real32_T;
-typedef double real64_T;
-
-/*===========================================================================*
- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
- * ulong_T, char_T and byte_T. *
- *===========================================================================*/
-
-typedef double real_T;
-typedef double time_T;
-typedef unsigned char boolean_T;
-typedef int int_T;
-typedef unsigned uint_T;
-typedef unsigned long ulong_T;
-typedef char char_T;
-typedef char_T byte_T;
-
-/*===========================================================================*
- * Complex number type definitions *
- *===========================================================================*/
-#define CREAL_T
- typedef struct {
- real32_T re;
- real32_T im;
- } creal32_T;
-
- typedef struct {
- real64_T re;
- real64_T im;
- } creal64_T;
-
- typedef struct {
- real_T re;
- real_T im;
- } creal_T;
-
- typedef struct {
- int8_T re;
- int8_T im;
- } cint8_T;
-
- typedef struct {
- uint8_T re;
- uint8_T im;
- } cuint8_T;
-
- typedef struct {
- int16_T re;
- int16_T im;
- } cint16_T;
-
- typedef struct {
- uint16_T re;
- uint16_T im;
- } cuint16_T;
-
- typedef struct {
- int32_T re;
- int32_T im;
- } cint32_T;
-
- typedef struct {
- uint32_T re;
- uint32_T im;
- } cuint32_T;
-
- typedef struct {
- int64_T re;
- int64_T im;
- } cint64_T;
-
- typedef struct {
- uint64_T re;
- uint64_T im;
- } cuint64_T;
-
-
-/*=======================================================================*
- * Min and Max: *
- * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
- * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
- *=======================================================================*/
-
-#define MAX_int8_T ((int8_T)(127))
-#define MIN_int8_T ((int8_T)(-128))
-#define MAX_uint8_T ((uint8_T)(255))
-#define MIN_uint8_T ((uint8_T)(0))
-#define MAX_int16_T ((int16_T)(32767))
-#define MIN_int16_T ((int16_T)(-32768))
-#define MAX_uint16_T ((uint16_T)(65535))
-#define MIN_uint16_T ((uint16_T)(0))
-#define MAX_int32_T ((int32_T)(2147483647))
-#define MIN_int32_T ((int32_T)(-2147483647-1))
-#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
-#define MIN_uint32_T ((uint32_T)(0))
-#define MAX_int64_T ((int64_T)(9223372036854775807L))
-#define MIN_int64_T ((int64_T)(-9223372036854775807L-1L))
-#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
-#define MIN_uint64_T ((uint64_T)(0UL))
-
-/* Logical type definitions */
-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
-# ifndef false
-# define false (0U)
-# endif
-# ifndef true
-# define true (1U)
-# endif
-#endif
-
-/*
- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
- * for signed integer values.
- */
-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
-#error "This code must be compiled using a 2's complement representation for signed integer values"
-#endif
-
-/*
- * Maximum length of a MATLAB identifier (function/variable)
- * including the null-termination character. Referenced by
- * rt_logging.c and rt_matrx.c.
- */
-#define TMW_NAME_LENGTH_MAX 64
-
-#endif
-#endif
-/* End of code generation (rtwtypes.h) */
diff --git a/.svn/pristine/9e/9e6e95514e91111f5fbd41abff3ed9f2920aa722.svn-base b/.svn/pristine/9e/9e6e95514e91111f5fbd41abff3ed9f2920aa722.svn-base
deleted file mode 100644
index eeb33ce3eec75dcdfa1229bbbb3843b3eaa29e87..0000000000000000000000000000000000000000
--- a/.svn/pristine/9e/9e6e95514e91111f5fbd41abff3ed9f2920aa722.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * expm.h
- *
- * Code generation for function 'expm'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __EXPM_H__
-#define __EXPM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_expm(real_T A[4], real_T F[4]);
-extern void expm(real_T A[9], real_T F[9]);
-#endif
-/* End of code generation (expm.h) */
diff --git a/.svn/pristine/9f/9f2731c8412ed16c272c4ebb1dfd0fbd56067909.svn-base b/.svn/pristine/9f/9f2731c8412ed16c272c4ebb1dfd0fbd56067909.svn-base
deleted file mode 100644
index 9fc09b3a06c2f657fd2aebe1fef355fb7e1d5143..0000000000000000000000000000000000000000
--- a/.svn/pristine/9f/9f2731c8412ed16c272c4ebb1dfd0fbd56067909.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * diag.h
- *
- * Code generation for function 'diag'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __DIAG_H__
-#define __DIAG_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_diag(const creal_T v[2], creal_T d[4]);
-extern void diag(const creal_T v[3], creal_T d[9]);
-#endif
-/* End of code generation (diag.h) */
diff --git a/.svn/pristine/a0/a055ae9133bb6f2351db128931fde942a5825374.svn-base b/.svn/pristine/a0/a055ae9133bb6f2351db128931fde942a5825374.svn-base
deleted file mode 100644
index 2138ad406089c3d93d11e13929a096e9d8cdcafa..0000000000000000000000000000000000000000
--- a/.svn/pristine/a0/a055ae9133bb6f2351db128931fde942a5825374.svn-base
+++ /dev/null
@@ -1,22 +0,0 @@
-<launch>
-
-
- <!-- Kinect Camera Transformation -->
- <node pkg="tf"
- type="static_transform_publisher"
- name="kinect"
- args="0.9014760 -0.0594 0.7251720 -0.6411 0.0380 0.7730 0.0043 /estirabot_link_base camera_link 100" >
- </node>
-
- <!-- Kinect -->
- <include file="$(find openni_launch)/launch/openni.launch" />
-
- <!-- iri_depth_tracker node -->
- <node name="iri_depth_tracker_node"
- pkg="iri_depth_tracker"
- type="depth_tracker" >
- </node>
-
-
-</launch>
-
diff --git a/.svn/pristine/aa/aacbd74f80df55d4e165ed63f294dfbfbd50eccf.svn-base b/.svn/pristine/aa/aacbd74f80df55d4e165ed63f294dfbfbd50eccf.svn-base
deleted file mode 100644
index b1a4a6247f9aedc5d99af6c9c0e556bf0d1d9796..0000000000000000000000000000000000000000
--- a/.svn/pristine/aa/aacbd74f80df55d4e165ed63f294dfbfbd50eccf.svn-base
+++ /dev/null
@@ -1,309 +0,0 @@
-/*
- *
- * Created on: May 2, 2013
- * Author: shusain
- */
-#include "ros/ros.h"
-#include "std_msgs/String.h"
-#include <pcl_ros/point_cloud.h>
-#include <sensor_msgs/PointCloud.h>
-#include <pcl/point_types.h>
-#include <boost/foreach.hpp>
-#include <boost/progress.hpp>
-#include <geometry_msgs/PoseStamped.h>
-
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/image_encodings.h>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/highgui/highgui.hpp>
-
-#include <iostream>
-#include <fstream>
-#include <boost/foreach.hpp>
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_initialize.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_terminate.h"
-
-#include "rt_nonfinite.h"
-#include "compute_prob1.h"
-//#include "LoadKinectMesh_realtime.h"
-#include "init_variables.h"
-
-#include <vector>
-#include <algorithm>
-//#include <omp.h>
-#define PI 3.141592653589793238462643383
-
-ros::Publisher g_pose_publisher;
-
-real_T Ixyz[504063];
-real_T corner_p[12];
-static real_T mean_img[3888];
-static real_T point_matrix[3888];
-static real_T AR_velocity[2250000];
-static real_T X_par[2250000];
-static real_T dw_dp[15552];
-static real_T tracked_images[38880000];
-static real_T X_par_pred[2250000];
-real_T centroid[3];
-int32_T t;
-real_T Aff_matrix[9];
-int32_T i0;
-int32_T i1;
-int32_T i2;
-int32_T row;
-int32_T column;
-int i;
-typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-real_T p[12];
-cv::Rect region_of_interest = cv::Rect(100, 50, 441, 381);
-cv::Mat color_img(480, 640, CV_8UC3);
-cv::Mat xyz_img(480, 640, CV_32FC3);
-cv::Mat color_img_roi;
-cv::Mat xyz_img_roi;
-// for video recording
-cv::Mat gray_img_roi;
-// end for video recording
-
-static float c_x=0,c_y=0;
-static float v_x=0,v_y=0;
-int mouse_loc_x[4];
-int mouse_loc_y[4];
-int mosue_click_time=0;
-real_T center_y;
-real_T center_x;
-double xx=0,yy=0,zz=0,dxy=0,xx_C=0,yy_C=0,zz_C=0,dxy2=0,xx2=0,yy2=0,zz2=0;
-
-void onMouse(int event, int x, int y, int flags, void* data)
-{
- if (event != cv::EVENT_LBUTTONDOWN)
- return;
- mouse_loc_x[mosue_click_time] = x;
- mouse_loc_y[mosue_click_time] = y;
-
- //printf("%d %d\n",x,y);
- printf("%d %d\n",mouse_loc_x[mosue_click_time],mouse_loc_y[mosue_click_time]);
- ++mosue_click_time;
-}
-
-
-void processPointCloud(const PointCloud::ConstPtr& msg) {
-
- boost::progress_timer timer;
- //static real_T mesh[921600];
- float x,y,z,ptrgb,yaw,pitch,roll;//,fr,fg,fb;
- uint8_t r,g,b;
- uint32_t rgb;
- i=0;
-
-// cvNamedWindow("color image",CV_WINDOW_NORMAL);
-// cvMoveWindow("color image",100,100);
- cvNamedWindow("range image");
-// cvMoveWindow("range image",545,100);
-/* cvNamedWindow("color image 2");
- cvMoveWindow("color image 2",100,100+415);
- cvNamedWindow("range image 2");
- cvMoveWindow("range image 2",545,100+415);
-*/
- BOOST_FOREACH (const pcl::PointXYZRGB& pt, msg->points)
- {
- ptrgb = pt.rgb;
- rgb = *reinterpret_cast<int*>(&ptrgb);
- r = (rgb >> 16) & 0x0000ff;
- g = (rgb >> 8) & 0x0000ff;
- b = (rgb) & 0x0000ff;
- x = (float)pt.x+2;
- y = (float)pt.y+2;
- z = (float)pt.z;
-
- if (x != x || y != y || z != z || z == 0)
- { x=0; y=0; z=0; r=0; g=0; b=0;
- }
- column = (int)(i / msg->width);
- row = i % (msg->width);
-
-/* color_img.at<cv::Vec3b>(column,row)[0] = b;
- color_img.at<cv::Vec3b>(column,row)[1] = g;
- color_img.at<cv::Vec3b>(column,row)[2] = r;*/
- xyz_img.at<cv::Vec3f>(column,row)[0] = x;
- xyz_img.at<cv::Vec3f>(column,row)[1] = y;
- xyz_img.at<cv::Vec3f>(column,row)[2] = z;
- ++i;
- }
-
-// color_img_roi = color_img(region_of_interest);
- xyz_img_roi = xyz_img(region_of_interest);
-
-// cv::cvtColor( color_img_roi, gray_img_roi, CV_RGB2GRAY );
-// cv::cvtColor( gray_img_roi, gray_img_roi, CV_GRAY2RGB );
-
- static int callback_counter_ = 0;
- int k,i3;
- if (callback_counter_ == 0) {
- /* % Object template initialization */
-//#pragma omp for schedule (dynamic, 8)
-#pragma omp parallel for
- for (k = 0; k < 441; k++) {
- for (i3 = 0; i3 < 381; i3++) {
- Ixyz[i3 + 381 * k] = xyz_img_roi.at<cv::Vec3f>(i3,k)[0];
- Ixyz[168021 + (i3 + 381 * k)] = xyz_img_roi.at<cv::Vec3f>(i3,k)[1];
- Ixyz[336042 + (i3 + 381 * k)] = xyz_img_roi.at<cv::Vec3f>(i3,k)[2];
- }
- }
- real_T ptx[4];
- real_T pty[4];
-
-
-// cv::imshow( "color image", color_img_roi );
- std::vector<cv::Mat> split_xyz(3,cv::Mat(xyz_img_roi.size(),CV_64FC1));
- cv::split(xyz_img_roi, split_xyz);
- static double min;
- static double max;
- cv::minMaxIdx(split_xyz.at(2), &min, &max);
- static cv::Mat adjMap;
- cv::convertScaleAbs(split_xyz.at(2), adjMap, 255 / max);
-
- cv::cvtColor( adjMap, adjMap, CV_GRAY2RGB );
- cv::imshow( "range image", adjMap );
- cv::setMouseCallback("range image", onMouse);
- ////////////////////for video recording
- //cv::imshow( "color image 2", gray_img_roi );
- //cv::imshow( "range image 2", adjMap );
- //////////////////// end for video recording
- cv::waitKey(0);
-
- ptx[0] = (double)mouse_loc_y[0];
- ptx[1] = (double)mouse_loc_y[1];
- ptx[2] = (double)mouse_loc_y[2];
- ptx[3] = (double)mouse_loc_y[3];
-
- pty[0] = (double)mouse_loc_x[0];
- pty[1] = (double)mouse_loc_x[1];
- pty[2] = (double)mouse_loc_x[2];
- pty[3] = (double)mouse_loc_x[3];
-
- init_variables(Ixyz, ptx, pty, X_par_pred, tracked_images, dw_dp, X_par,
- AR_velocity, point_matrix, mean_img, corner_p, ¢er_x,
- ¢er_y);
- ++callback_counter_;
-
- }
- else {
-
- for (k = 0; k < 441; k++) {
- for (i3 = 0; i3 < 381; i3++) {
- Ixyz[i3 + 381 * k] = xyz_img_roi.at<cv::Vec3f>(i3,k)[0];
- Ixyz[168021 + (i3 + 381 * k)] = xyz_img_roi.at<cv::Vec3f>(i3,k)[1];
- Ixyz[336042 + (i3 + 381 * k)] = xyz_img_roi.at<cv::Vec3f>(i3,k)[2];
- }
- }
- /* %% Particle propagation and likelihood computation */
- compute_prob1(X_par, X_par_pred, AR_velocity, dw_dp, (real_T)t + 2.0,
- center_x, center_y, Ixyz, point_matrix, mean_img,
- tracked_images, Aff_matrix, centroid);
- ++t;
- /* %% Display the tracking results */
-//#pragma omp for// schedule (dynamic, 8)
- for (i0 = 0; i0 < 3; i0++) {
- for (i1 = 0; i1 < 4; i1++) {
- p[i0 + 3 * i1] = 0.0;
- for (i2 = 0; i2 < 3; i2++) {
- p[i0 + 3 * i1] += Aff_matrix[i0 + 3 * i2] * corner_p[i2 + 3 * i1];
- }
- }
- }
- ++callback_counter_;
- }
-
- geometry_msgs::PoseStamped pose_stamped;
- //pose_stamped.header.stamp.sec = callback_counter_;
- pose_stamped.header.frame_id = "/camera_rgb_optical_frame";
-
- ////////////////////for video recording
- //cv::imshow( "color image 2", gray_img_roi );
- //////////////////// end for video recording
-
- // extract the centroid of the 4 corners of the bounding box
- c_x = (p[4]+p[7]+p[10]+p[1])/4;
- c_y = (p[3]+p[6]+p[9]+p[0])/4;
-
-/* cv::line(color_img_roi, cv::Point(p[4],p[3]), cv::Point(p[1],p[0]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(color_img_roi, cv::Point(p[7],p[6]), cv::Point(p[4],p[3]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(color_img_roi, cv::Point(p[10],p[9]), cv::Point(p[7],p[6]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(color_img_roi, cv::Point(p[1],p[0]), cv::Point(p[10],p[9]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(color_img_roi, cv::Point(c_x,c_y), cv::Point((p[7]+p[4])/2,(p[6]+p[3])/2), cv::Scalar(0, 0, 255), 2, CV_AA);*/
-
- v_x = c_x-((p[7]+p[4])/2);
- v_y = c_y-((p[6]+p[3])/2);
-
- //
-// cv::imshow( "color image", color_img_roi );
- // show depth image
- std::vector<cv::Mat> split_xyz(3,cv::Mat(xyz_img_roi.size(),CV_64FC1));
- cv::split(xyz_img_roi, split_xyz);
- static double min;
- static double max;
- cv::minMaxIdx(split_xyz.at(2), &min, &max);
- static cv::Mat adjMap;
- cv::convertScaleAbs(split_xyz.at(2), adjMap, 255 / max);
- cv::cvtColor( adjMap, adjMap, CV_GRAY2RGB );
- ////////////////////for video recording
- //cv::imshow( "range image 2", adjMap );
- //////////////////// end for video recording
- cv::line(adjMap, cv::Point(p[4],p[3]), cv::Point(p[1],p[0]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(adjMap, cv::Point(p[7],p[6]), cv::Point(p[4],p[3]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(adjMap, cv::Point(p[10],p[9]), cv::Point(p[7],p[6]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(adjMap, cv::Point(p[1],p[0]), cv::Point(p[10],p[9]), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(adjMap, cv::Point(c_x,c_y), cv::Point((p[7]+p[4])/2,(p[6]+p[3])/2), cv::Scalar(0, 0, 255), 2, CV_AA);
- cv::line(adjMap, cv::Point(c_x,c_y), cv::Point((p[1]+p[4])/2,(p[0]+p[3])/2), cv::Scalar(0, 255, 0), 2, CV_AA);
- cv::imshow( "range image", adjMap );
- cv::waitKey(1);
-
-
- pose_stamped.pose.position.x = centroid[0]-2;
- pose_stamped.pose.position.y = centroid[1]-2;
- pose_stamped.pose.position.z = centroid[2];
- // compute image plane rotation
- pose_stamped.pose.orientation.y = atan(v_y/v_x);
- //yaw = atan(v_y/v_x);
-
- // compute out of plane rotation
- cv::Point pt_C((p[4]+p[7]+p[10]+p[1])/4,(p[3]+p[6]+p[9]+p[0])/4);
- xx_C = xyz_img_roi.at<cv::Vec3f>(pt_C)[0];
- yy_C = xyz_img_roi.at<cv::Vec3f>(pt_C)[1];
- zz_C = xyz_img_roi.at<cv::Vec3f>(pt_C)[2];
- cv::Point pt((p[7]+p[4])/2,(p[6]+p[3])/2);
- xx = xyz_img_roi.at<cv::Vec3f>(pt)[0];
- yy = xyz_img_roi.at<cv::Vec3f>(pt)[1];
- zz = xyz_img_roi.at<cv::Vec3f>(pt)[2];
- dxy = pow(xx-xx_C,2)+pow(yy-yy_C,2);
- //pitch = atan((zz-zz_C)/(sqrt(dxy)));
- pose_stamped.pose.orientation.x = atan((zz-zz_C)/(sqrt(dxy)));
- //pose_stamped.pose.orientation.y = atan(zz-zz_C)*180/PI;
- //roll = 0;
- cv::Point pt2((p[1]+p[4])/2,(p[0]+p[3])/2);
- xx2 = xyz_img_roi.at<cv::Vec3f>(pt2)[0];
- yy2 = xyz_img_roi.at<cv::Vec3f>(pt2)[1];
- zz2 = xyz_img_roi.at<cv::Vec3f>(pt2)[2];
- dxy2 = pow(xx2-xx_C,2)+pow(yy2-yy_C,2);
- pose_stamped.pose.orientation.z = atan((zz2-zz_C)/(sqrt(dxy2)));
-
- g_pose_publisher.publish(pose_stamped);
-
-}
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "depth_tracker");
- ros::NodeHandle n;
- Optimal_affine_tracking_3d16_fast_realtime_initialize();
- g_pose_publisher = n.advertise<geometry_msgs::PoseStamped>("pose_surface", 50);
- //std::cout << "Number processors: " << omp_get_num_procs() << std::endl;
- /* % Execute tracking */
- ros::Subscriber sub = n.subscribe<PointCloud>("/camera/depth_registered/points", 5, processPointCloud);
-
- Optimal_affine_tracking_3d16_fast_realtime_terminate();
- ros::spin();
- return 0;
-}
diff --git a/.svn/pristine/ac/aca8f87b79c387e7e288f04ecca8562c138f9de0.svn-base b/.svn/pristine/ac/aca8f87b79c387e7e288f04ecca8562c138f9de0.svn-base
deleted file mode 100644
index 413e0269a593ace6354e20b4b64b2f2c90c596f7..0000000000000000000000000000000000000000
--- a/.svn/pristine/ac/aca8f87b79c387e7e288f04ecca8562c138f9de0.svn-base
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * mrdivide.h
- *
- * Code generation for function 'mrdivide'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __MRDIVIDE_H__
-#define __MRDIVIDE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_mrdivide(const creal_T A[9], const creal_T B[9], creal_T y[9]);
-extern creal_T c_eml_div(const creal_T x, const creal_T y);
-extern void c_mrdivide(const creal_T A[4], const creal_T B[4], creal_T y[4]);
-extern void mrdivide(const real_T A[12], const real_T B[12], real_T y[9]);
-#endif
-/* End of code generation (mrdivide.h) */
diff --git a/.svn/pristine/ac/acbb65f370d018482e35a40fe9b9c3d5d84eb4eb.svn-base b/.svn/pristine/ac/acbb65f370d018482e35a40fe9b9c3d5d84eb4eb.svn-base
deleted file mode 100644
index fec4a294b603f21937b9637bf6182c43f8f96f1e..0000000000000000000000000000000000000000
--- a/.svn/pristine/ac/acbb65f370d018482e35a40fe9b9c3d5d84eb4eb.svn-base
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * rtGetInf.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/*
- * Abstract:
- * MATLAB for code generation function to initialize non-finite, Inf and MinusInf
- */
-#include "rtGetInf.h"
-#define NumBitsPerChar 8U
-
-/* Function: rtGetInf ==================================================
- * Abstract:
- * Initialize rtInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetInf(void)
-{
- size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
- real_T inf = 0.0;
- if (bitsPerReal == 32U) {
- inf = rtGetInfF();
- } else {
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- union {
- LittleEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0x7FF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- inf = tmpVal.fltVal;
- break;
- }
-
- case BigEndian:
- {
- union {
- BigEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0x7FF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- inf = tmpVal.fltVal;
- break;
- }
- }
- }
-
- return inf;
-}
-
-/* Function: rtGetInfF ==================================================
- * Abstract:
- * Initialize rtInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetInfF(void)
-{
- IEEESingle infF;
- infF.wordL.wordLuint = 0x7F800000U;
- return infF.wordL.wordLreal;
-}
-
-/* Function: rtGetMinusInf ==================================================
- * Abstract:
- * Initialize rtMinusInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetMinusInf(void)
-{
- size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
- real_T minf = 0.0;
- if (bitsPerReal == 32U) {
- minf = rtGetMinusInfF();
- } else {
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- union {
- LittleEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0xFFF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- minf = tmpVal.fltVal;
- break;
- }
-
- case BigEndian:
- {
- union {
- BigEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0xFFF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- minf = tmpVal.fltVal;
- break;
- }
- }
- }
-
- return minf;
-}
-
-/* Function: rtGetMinusInfF ==================================================
- * Abstract:
- * Initialize rtMinusInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetMinusInfF(void)
-{
- IEEESingle minfF;
- minfF.wordL.wordLuint = 0xFF800000U;
- return minfF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetInf.cpp) */
diff --git a/.svn/pristine/b3/b3d99fbdf2484b2da3c27ca8f76612b272143259.svn-base b/.svn/pristine/b3/b3d99fbdf2484b2da3c27ca8f76612b272143259.svn-base
deleted file mode 100644
index ec22d9600a4c364874c137ffd1c51b128ec5474c..0000000000000000000000000000000000000000
--- a/.svn/pristine/b3/b3d99fbdf2484b2da3c27ca8f76612b272143259.svn-base
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * quat2rot.cpp
- *
- * Code generation for function 'quat2rot'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "quat2rot.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void quat2rot(const real_T Q[4], real_T R[9])
-{
- /* QUAT2ROT */
- /* R = QUAT2ROT(Q) converts a quaternion (4x1 or 1x4) into a 3x3 rotation mattrix */
- /* */
- /* reference: google! */
- /* */
- /* Babak Taati, 2003 */
- /* (revised 2009) */
- R[0] = ((Q[0] * Q[0] + Q[1] * Q[1]) - Q[2] * Q[2]) - Q[3] * Q[3];
- R[3] = 2.0 * (Q[1] * Q[2] - Q[0] * Q[3]);
- R[6] = 2.0 * (Q[1] * Q[3] + Q[0] * Q[2]);
- R[1] = 2.0 * (Q[1] * Q[2] + Q[0] * Q[3]);
- R[4] = ((Q[0] * Q[0] - Q[1] * Q[1]) + Q[2] * Q[2]) - Q[3] * Q[3];
- R[7] = 2.0 * (Q[2] * Q[3] - Q[0] * Q[1]);
- R[2] = 2.0 * (Q[1] * Q[3] - Q[0] * Q[2]);
- R[5] = 2.0 * (Q[2] * Q[3] + Q[0] * Q[1]);
- R[8] = ((Q[0] * Q[0] - Q[1] * Q[1]) - Q[2] * Q[2]) + Q[3] * Q[3];
-}
-
-/* End of code generation (quat2rot.cpp) */
diff --git a/.svn/pristine/b9/b98742e0eceeb554afc3b7dd10501d97e2764802.svn-base b/.svn/pristine/b9/b98742e0eceeb554afc3b7dd10501d97e2764802.svn-base
deleted file mode 100644
index b7258ba83f1170a9447b3d49cc238d12f18ab078..0000000000000000000000000000000000000000
--- a/.svn/pristine/b9/b98742e0eceeb554afc3b7dd10501d97e2764802.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * cumulative_sum.h
- *
- * Code generation for function 'cumulative_sum'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __CUMULATIVE_SUM_H__
-#define __CUMULATIVE_SUM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void cumulative_sum(const real_T x[25], real_T csum[25]);
-#endif
-/* End of code generation (cumulative_sum.h) */
diff --git a/.svn/pristine/ba/ba31507eb9fad36055ac32e80556f8e8ad48ea25.svn-base b/.svn/pristine/ba/ba31507eb9fad36055ac32e80556f8e8ad48ea25.svn-base
deleted file mode 100644
index 0f5aac917e8fc8d55a016982c319945d4bd81072..0000000000000000000000000000000000000000
--- a/.svn/pristine/ba/ba31507eb9fad36055ac32e80556f8e8ad48ea25.svn-base
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * rt_nonfinite.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/*
- * Abstract:
- * MATLAB for code generation function to initialize non-finites,
- * (Inf, NaN and -Inf).
- */
-#include "rt_nonfinite.h"
-#include "rtGetNaN.h"
-#include "rtGetInf.h"
-
-real_T rtInf;
-real_T rtMinusInf;
-real_T rtNaN;
-real32_T rtInfF;
-real32_T rtMinusInfF;
-real32_T rtNaNF;
-
-/* Function: rt_InitInfAndNaN ==================================================
- * Abstract:
- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
- * generated code. NaN is initialized as non-signaling. Assumes IEEE.
- */
-void rt_InitInfAndNaN(size_t realSize)
-{
- (void) (realSize);
- rtNaN = rtGetNaN();
- rtNaNF = rtGetNaNF();
- rtInf = rtGetInf();
- rtInfF = rtGetInfF();
- rtMinusInf = rtGetMinusInf();
- rtMinusInfF = rtGetMinusInfF();
-}
-
-/* Function: rtIsInf ==================================================
- * Abstract:
- * Test if value is infinite
- */
-boolean_T rtIsInf(real_T value)
-{
- return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
-}
-
-/* Function: rtIsInfF =================================================
- * Abstract:
- * Test if single-precision value is infinite
- */
-boolean_T rtIsInfF(real32_T value)
-{
- return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
-}
-
-/* Function: rtIsNaN ==================================================
- * Abstract:
- * Test if value is not a number
- */
-boolean_T rtIsNaN(real_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
- return _isnan(value)? TRUE:FALSE;
-#else
- return (value!=value)? 1U:0U;
-#endif
-}
-
-/* Function: rtIsNaNF =================================================
- * Abstract:
- * Test if single-precision value is not a number
- */
-boolean_T rtIsNaNF(real32_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
- return _isnan((real_T)value)? true:false;
-#else
- return (value!=value)? 1U:0U;
-#endif
-}
-
-
-/* End of code generation (rt_nonfinite.cpp) */
diff --git a/.svn/pristine/bb/bb490cb6ca000c7d082f43bd23405d5b6ebcc4eb.svn-base b/.svn/pristine/bb/bb490cb6ca000c7d082f43bd23405d5b6ebcc4eb.svn-base
deleted file mode 100644
index ef0c9aa7c3e2aebe29446e2dd06554c1d86d0aaf..0000000000000000000000000000000000000000
--- a/.svn/pristine/bb/bb490cb6ca000c7d082f43bd23405d5b6ebcc4eb.svn-base
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * sqrt.cpp
- *
- * Code generation for function 'sqrt'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "sqrt.h"
-#include "log.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_sqrt(creal_T *x)
-{
- real_T absxi;
- real_T absxr;
- if (x->im == 0.0) {
- if (x->re < 0.0) {
- absxi = 0.0;
- absxr = sqrt(fabs(x->re));
- } else {
- absxi = sqrt(x->re);
- absxr = 0.0;
- }
- } else if (x->re == 0.0) {
- if (x->im < 0.0) {
- absxi = sqrt(-x->im / 2.0);
- absxr = -absxi;
- } else {
- absxi = sqrt(x->im / 2.0);
- absxr = absxi;
- }
- } else if (rtIsNaN(x->re) || rtIsNaN(x->im)) {
- absxi = rtNaN;
- absxr = rtNaN;
- } else if (rtIsInf(x->im)) {
- absxi = rtInf;
- absxr = x->im;
- } else if (rtIsInf(x->re)) {
- if (x->re < 0.0) {
- absxi = 0.0;
- absxr = rtInf;
- } else {
- absxi = rtInf;
- absxr = 0.0;
- }
- } else {
- absxr = fabs(x->re);
- absxi = fabs(x->im);
- if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
- {
- absxr *= 0.5;
- absxi *= 0.5;
- absxi = rt_hypotd_snf(absxr, absxi);
- if (absxi > absxr) {
- absxi = sqrt(absxi) * sqrt(1.0 + absxr / absxi);
- } else {
- absxi = sqrt(absxi) * 1.4142135623730951;
- }
- } else {
- absxi = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
- }
-
- if (x->re > 0.0) {
- absxr = 0.5 * (x->im / absxi);
- } else {
- if (x->im < 0.0) {
- absxr = -absxi;
- } else {
- absxr = absxi;
- }
-
- absxi = 0.5 * (x->im / absxr);
- }
- }
-
- x->re = absxi;
- x->im = absxr;
-}
-
-/* End of code generation (sqrt.cpp) */
diff --git a/.svn/pristine/bd/bdedb43a7bab184de6b11f873e8aa3ef05e9fd63.svn-base b/.svn/pristine/bd/bdedb43a7bab184de6b11f873e8aa3ef05e9fd63.svn-base
deleted file mode 100644
index c60be01c7acdd7e852880796e2b79f32ef43bfa0..0000000000000000000000000000000000000000
--- a/.svn/pristine/bd/bdedb43a7bab184de6b11f873e8aa3ef05e9fd63.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * eig.h
- *
- * Code generation for function 'eig'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __EIG_H__
-#define __EIG_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_eig(const real_T A[4], creal_T V[4], creal_T D[4]);
-extern void eig(const real_T A[9], creal_T V[9], creal_T D[9]);
-#endif
-/* End of code generation (eig.h) */
diff --git a/.svn/pristine/bf/bf0caa5f11e9cb6506a159647fc62f6f57a5a87d.svn-base b/.svn/pristine/bf/bf0caa5f11e9cb6506a159647fc62f6f57a5a87d.svn-base
deleted file mode 100644
index bd8208908b1893ce724915dfe14e919ed2804403..0000000000000000000000000000000000000000
--- a/.svn/pristine/bf/bf0caa5f11e9cb6506a159647fc62f6f57a5a87d.svn-base
+++ /dev/null
@@ -1,220 +0,0 @@
-/*
- * resampling.cpp
- *
- * Code generation for function 'resampling'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "resampling.h"
-#include "meshgrid.h"
-#include "cumulative_sum.h"
-#include "rand.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void resampling(const real_T w[25], real_T outindex[25])
-{
- real_T ndbl;
- real_T y;
- int32_T xtmp;
- real_T apnd;
- real_T absa;
- real_T b_absa;
- real_T U_data[26];
- int32_T nm1d2;
- int32_T k;
- real_T dv10[25];
- real_T cum[26];
- real_T dv11[25];
- real_T dv12[25];
- int32_T i;
- real_T jj[625];
- real_T ii[625];
- int32_T i16;
- boolean_T cond[625];
- int32_T ind;
- int32_T b_ind;
- int16_T ii_data[1];
- boolean_T exitg1;
- int16_T tmp_data[625];
- int32_T loop_ub;
- int32_T i17;
- int16_T b_tmp_data[625];
- int16_T b_ii_data[1];
- int16_T j_data[1];
- ndbl = c_rand();
- y = ndbl / 25.0;
- if (rtIsNaN(y)) {
- xtmp = 0;
- y = rtNaN;
- apnd = 1.0;
- } else if (rtIsInf(y)) {
- xtmp = 0;
- y = rtNaN;
- apnd = 1.0;
- } else {
- ndbl = floor((1.0 - y) / 0.04 + 0.5);
- apnd = y + ndbl * 0.04;
- absa = fabs(y);
- if (absa > 1.0) {
- b_absa = absa;
- } else {
- b_absa = 1.0;
- }
-
- if (fabs(apnd - 1.0) < 4.4408920985006262E-16 * b_absa) {
- ndbl++;
- apnd = 1.0;
- } else if (apnd - 1.0 > 0.0) {
- apnd = y + (ndbl - 1.0) * 0.04;
- } else {
- ndbl++;
- }
-
- xtmp = (int32_T)ndbl - 1;
- }
-
- U_data[0] = y;
- if (xtmp + 1 > 1) {
- U_data[xtmp] = apnd;
- nm1d2 = xtmp / 2;
- for (k = 1; k < nm1d2; k++) {
- ndbl = (real_T)k * 0.04;
- U_data[k] = y + ndbl;
- U_data[xtmp - k] = apnd - ndbl;
- }
-
- if (nm1d2 << 1 == xtmp) {
- U_data[nm1d2] = (y + apnd) / 2.0;
- } else {
- ndbl = (real_T)nm1d2 * 0.04;
- U_data[nm1d2] = y + ndbl;
- U_data[nm1d2 + 1] = apnd - ndbl;
- }
- }
-
- cumulative_sum(w, dv10);
- cum[0] = 0.0;
- for (i = 0; i < 25; i++) {
- cum[i + 1] = dv10[i];
- outindex[i] = 0.0;
- dv11[i] = 1.0 + (real_T)i;
- dv12[i] = 2.0 + (real_T)i;
- }
-
- meshgrid(dv11, dv12, ii, jj);
- for (i16 = 0; i16 < 625; i16++) {
- cond[i16] = ((U_data[(int32_T)ii[i16] - 1] > cum[(int32_T)(jj[i16] - 1.0) -
- 1]) && (U_data[(int32_T)ii[i16] - 1] <= cum[(int32_T)jj[i16] -
- 1]));
- }
-
- i = 0;
- for (ind = 0; ind < 25; ind++) {
- b_ind = 1 + ind * 25;
- if (b_ind > b_ind + 24) {
- b_ind = 1;
- i16 = 0;
- } else {
- i16 = b_ind + 24;
- }
-
- xtmp = (i16 - b_ind) + 1;
- if (1 <= xtmp) {
- k = 1;
- } else {
- k = xtmp;
- }
-
- nm1d2 = 0;
- xtmp = (i16 - b_ind) + 1;
- exitg1 = FALSE;
- while ((exitg1 == FALSE) && (xtmp > 0)) {
- loop_ub = i16 - b_ind;
- for (i17 = 0; i17 <= loop_ub; i17++) {
- tmp_data[i17] = (int16_T)(b_ind + i17);
- }
-
- loop_ub = (i16 - b_ind) + 1;
- for (i17 = 0; i17 < loop_ub; i17++) {
- b_tmp_data[i17] = tmp_data[i17];
- }
-
- if (cond[b_tmp_data[xtmp - 1] - 1]) {
- nm1d2 = 1;
- ii_data[0] = (int16_T)xtmp;
- exitg1 = TRUE;
- } else {
- xtmp--;
- }
- }
-
- if (k == 1) {
- if (nm1d2 == 0) {
- k = 0;
- }
- } else {
- if (1 > nm1d2) {
- loop_ub = -1;
- } else {
- loop_ub = 0;
- }
-
- xtmp = loop_ub + 1;
- i16 = 0;
- while (i16 <= xtmp - 1) {
- i16 = 0;
- while (i16 <= 0) {
- b_ii_data[0] = ii_data[0];
- i16 = 1;
- }
-
- i16 = 1;
- }
-
- k = loop_ub + 1;
- xtmp = loop_ub + 1;
- i16 = 0;
- while (i16 <= xtmp - 1) {
- ii_data[0] = b_ii_data[0];
- i16 = 1;
- }
-
- i16 = loop_ub + 1;
- nm1d2 = i16 / 2;
- xtmp = 1;
- while (xtmp <= nm1d2) {
- xtmp = ii_data[0];
- ii_data[0] = ii_data[loop_ub];
- ii_data[loop_ub] = (int16_T)xtmp;
- xtmp = 2;
- }
- }
-
- loop_ub = k;
- i16 = 0;
- while (i16 <= loop_ub - 1) {
- j_data[0] = ii_data[0];
- i16 = 1;
- }
-
- outindex[i] = (real_T)j_data[0];
- i++;
- }
-}
-
-/* End of code generation (resampling.cpp) */
diff --git a/.svn/pristine/c2/c2ba17861379e8412430242b96ead4fe3d2c0652.svn-base b/.svn/pristine/c2/c2ba17861379e8412430242b96ead4fe3d2c0652.svn-base
deleted file mode 100644
index 6e00afe917ed8f16dd1e6467ab271a87cf9300d6..0000000000000000000000000000000000000000
--- a/.svn/pristine/c2/c2ba17861379e8412430242b96ead4fe3d2c0652.svn-base
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * sum.cpp
- *
- * Code generation for function 'sum'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "sum.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void b_sum(const real_T x[7776], real_T y[6])
-{
- int32_T ix;
- int32_T iy;
- int32_T i;
- int32_T ixstart;
- real_T s;
- ix = -1;
- iy = -1;
- for (i = 0; i < 6; i++) {
- ixstart = ix + 1;
- ix++;
- s = x[ixstart];
- for (ixstart = 0; ixstart < 1295; ixstart++) {
- ix++;
- s += x[ix];
- }
-
- iy++;
- y[iy] = s;
- }
-}
-
-real_T c_sum(const real_T x[25])
-{
- real_T y;
- int32_T k;
- y = x[0];
- for (k = 0; k < 24; k++) {
- y += x[k + 1];
- }
-
- return y;
-}
-
-void sum(const real_T x[3888], real_T y[3])
-{
- int32_T iy;
- int32_T ixstart;
- int32_T j;
- int32_T ix;
- real_T s;
- int32_T k;
- iy = -1;
- ixstart = -1;
- for (j = 0; j < 3; j++) {
- ixstart++;
- ix = ixstart;
- s = x[ixstart];
- for (k = 0; k < 1295; k++) {
- ix += 3;
- s += x[ix];
- }
-
- iy++;
- y[iy] = s;
- }
-}
-
-/* End of code generation (sum.cpp) */
diff --git a/.svn/pristine/c3/c3151a1d70b058c810c39afad3dda79684f63393.svn-base b/.svn/pristine/c3/c3151a1d70b058c810c39afad3dda79684f63393.svn-base
deleted file mode 100644
index 40b5ed438ece7a5f9aa681e56017b372e1afbf34..0000000000000000000000000000000000000000
--- a/.svn/pristine/c3/c3151a1d70b058c810c39afad3dda79684f63393.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_initialize.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_initialize'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_INITIALIZE_H__
-#define __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void Optimal_affine_tracking_3d16_fast_realtime_initialize(void);
-#endif
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_initialize.h) */
diff --git a/.svn/pristine/c6/c6663abd9e18e1949c8e3c747fda1b5606538cb6.svn-base b/.svn/pristine/c6/c6663abd9e18e1949c8e3c747fda1b5606538cb6.svn-base
deleted file mode 100644
index b5f2eceb9d96b6472376e7258cb4d6395b3db5d1..0000000000000000000000000000000000000000
--- a/.svn/pristine/c6/c6663abd9e18e1949c8e3c747fda1b5606538cb6.svn-base
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_types.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_TYPES_H__
-#define __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_TYPES_H__
-
-/* Type Definitions */
-#ifndef struct_emxArray_int32_T_1
-#define struct_emxArray_int32_T_1
-struct emxArray_int32_T_1
-{
- int32_T data[1];
- int32_T size[1];
-};
-#endif /*struct_emxArray_int32_T_1*/
-#ifndef typedef_emxArray_int32_T_1
-#define typedef_emxArray_int32_T_1
-typedef struct emxArray_int32_T_1 emxArray_int32_T_1;
-#endif /*typedef_emxArray_int32_T_1*/
-#ifndef struct_emxArray_int32_T_1296
-#define struct_emxArray_int32_T_1296
-struct emxArray_int32_T_1296
-{
- int32_T data[1296];
- int32_T size[1];
-};
-#endif /*struct_emxArray_int32_T_1296*/
-#ifndef typedef_emxArray_int32_T_1296
-#define typedef_emxArray_int32_T_1296
-typedef struct emxArray_int32_T_1296 emxArray_int32_T_1296;
-#endif /*typedef_emxArray_int32_T_1296*/
-#ifndef struct_emxArray_int32_T_1x1
-#define struct_emxArray_int32_T_1x1
-struct emxArray_int32_T_1x1
-{
- int32_T data[1];
- int32_T size[2];
-};
-#endif /*struct_emxArray_int32_T_1x1*/
-#ifndef typedef_emxArray_int32_T_1x1
-#define typedef_emxArray_int32_T_1x1
-typedef struct emxArray_int32_T_1x1 emxArray_int32_T_1x1;
-#endif /*typedef_emxArray_int32_T_1x1*/
-#ifndef struct_emxArray_int32_T_25
-#define struct_emxArray_int32_T_25
-struct emxArray_int32_T_25
-{
- int32_T data[25];
- int32_T size[1];
-};
-#endif /*struct_emxArray_int32_T_25*/
-#ifndef typedef_emxArray_int32_T_25
-#define typedef_emxArray_int32_T_25
-typedef struct emxArray_int32_T_25 emxArray_int32_T_25;
-#endif /*typedef_emxArray_int32_T_25*/
-#ifndef struct_emxArray_int32_T_625
-#define struct_emxArray_int32_T_625
-struct emxArray_int32_T_625
-{
- int32_T data[625];
- int32_T size[1];
-};
-#endif /*struct_emxArray_int32_T_625*/
-#ifndef typedef_emxArray_int32_T_625
-#define typedef_emxArray_int32_T_625
-typedef struct emxArray_int32_T_625 emxArray_int32_T_625;
-#endif /*typedef_emxArray_int32_T_625*/
-#ifndef struct_emxArray_real_T_1296
-#define struct_emxArray_real_T_1296
-struct emxArray_real_T_1296
-{
- real_T data[1296];
- int32_T size[1];
-};
-#endif /*struct_emxArray_real_T_1296*/
-#ifndef typedef_emxArray_real_T_1296
-#define typedef_emxArray_real_T_1296
-typedef struct emxArray_real_T_1296 emxArray_real_T_1296;
-#endif /*typedef_emxArray_real_T_1296*/
-#ifndef struct_emxArray_real_T_1x26
-#define struct_emxArray_real_T_1x26
-struct emxArray_real_T_1x26
-{
- real_T data[26];
- int32_T size[2];
-};
-#endif /*struct_emxArray_real_T_1x26*/
-#ifndef typedef_emxArray_real_T_1x26
-#define typedef_emxArray_real_T_1x26
-typedef struct emxArray_real_T_1x26 emxArray_real_T_1x26;
-#endif /*typedef_emxArray_real_T_1x26*/
-
-#endif
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_types.h) */
diff --git a/.svn/pristine/c6/c6fd932b1e322eb72086e56900044190ff61831e.svn-base b/.svn/pristine/c6/c6fd932b1e322eb72086e56900044190ff61831e.svn-base
deleted file mode 100644
index a581d68d032c7a3d9e16f25b3ef48e82f54f6fc1..0000000000000000000000000000000000000000
--- a/.svn/pristine/c6/c6fd932b1e322eb72086e56900044190ff61831e.svn-base
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * log.cpp
- *
- * Code generation for function 'log'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "log.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static void eml_scalar_log(creal_T *x);
-static real_T rt_atan2d_snf(real_T u0, real_T u1);
-
-/* Function Definitions */
-static void eml_scalar_log(creal_T *x)
-{
- real_T x_re;
- real_T x_im;
- real_T b_x_im;
- real_T b_x_re;
- if ((x->im == 0.0) && rtIsNaN(x->re)) {
- } else if ((fabs(x->re) > 8.9884656743115785E+307) || (fabs(x->im) >
- 8.9884656743115785E+307)) {
- x_re = x->re;
- x_im = x->im;
- b_x_im = x->im;
- b_x_re = x->re;
- x->re = log(rt_hypotd_snf(fabs(x_re / 2.0), fabs(x_im / 2.0))) +
- 0.69314718055994529;
- x->im = rt_atan2d_snf(b_x_im, b_x_re);
- } else {
- x_re = x->re;
- x_im = x->im;
- b_x_im = x->im;
- b_x_re = x->re;
- x->re = log(rt_hypotd_snf(fabs(x_re), fabs(x_im)));
- x->im = rt_atan2d_snf(b_x_im, b_x_re);
- }
-}
-
-static real_T rt_atan2d_snf(real_T u0, real_T u1)
-{
- real_T y;
- int32_T b_u0;
- int32_T b_u1;
- if (rtIsNaN(u0) || rtIsNaN(u1)) {
- y = rtNaN;
- } else if (rtIsInf(u0) && rtIsInf(u1)) {
- if (u0 > 0.0) {
- b_u0 = 1;
- } else {
- b_u0 = -1;
- }
-
- if (u1 > 0.0) {
- b_u1 = 1;
- } else {
- b_u1 = -1;
- }
-
- y = atan2((real_T)b_u0, (real_T)b_u1);
- } else if (u1 == 0.0) {
- if (u0 > 0.0) {
- y = RT_PI / 2.0;
- } else if (u0 < 0.0) {
- y = -(RT_PI / 2.0);
- } else {
- y = 0.0;
- }
- } else {
- y = atan2(u0, u1);
- }
-
- return y;
-}
-
-void b_log(creal_T x[3])
-{
- int32_T k;
- for (k = 0; k < 3; k++) {
- eml_scalar_log(&x[k]);
- }
-}
-
-void c_log(creal_T x[2])
-{
- int32_T k;
- for (k = 0; k < 2; k++) {
- eml_scalar_log(&x[k]);
- }
-}
-
-/* End of code generation (log.cpp) */
diff --git a/.svn/pristine/c7/c7e96ab26f242361788b4c430c99c7c122d63111.svn-base b/.svn/pristine/c7/c7e96ab26f242361788b4c430c99c7c122d63111.svn-base
deleted file mode 100644
index 2dee8898d4a7a4890681f455ea069f7d3737efd7..0000000000000000000000000000000000000000
--- a/.svn/pristine/c7/c7e96ab26f242361788b4c430c99c7c122d63111.svn-base
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * round.cpp
- *
- * Code generation for function 'round'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "round.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real_T rt_roundd_snf(real_T u);
-
-/* Function Definitions */
-static real_T rt_roundd_snf(real_T u)
-{
- real_T y;
- if (fabs(u) < 4.503599627370496E+15) {
- if (u >= 0.5) {
- y = floor(u + 0.5);
- } else if (u > -0.5) {
- y = -0.0;
- } else {
- y = ceil(u - 0.5);
- }
- } else {
- y = u;
- }
-
- return y;
-}
-
-void b_round(real_T x[1296])
-{
- int32_T k;
- for (k = 0; k < 1296; k++) {
- x[k] = rt_roundd_snf(x[k]);
- }
-}
-
-/* End of code generation (round.cpp) */
diff --git a/.svn/pristine/cb/cb5884aec8eb3ad116ba0aafcc18a90b08aae88e.svn-base b/.svn/pristine/cb/cb5884aec8eb3ad116ba0aafcc18a90b08aae88e.svn-base
deleted file mode 100644
index a8febe6c917f2de8ce723fa054489ef92afd0a24..0000000000000000000000000000000000000000
--- a/.svn/pristine/cb/cb5884aec8eb3ad116ba0aafcc18a90b08aae88e.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * randn.h
- *
- * Code generation for function 'randn'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RANDN_H__
-#define __RANDN_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real_T genrandu(uint32_T mt[625]);
-extern void randn(real_T r[6]);
-#endif
-/* End of code generation (randn.h) */
diff --git a/.svn/pristine/cc/cca09c8c06e1f09feb66d5c349d5984645f4e94c.svn-base b/.svn/pristine/cc/cca09c8c06e1f09feb66d5c349d5984645f4e94c.svn-base
deleted file mode 100644
index 1f227617017082d73aba6f6681ffb47067c9b598..0000000000000000000000000000000000000000
--- a/.svn/pristine/cc/cca09c8c06e1f09feb66d5c349d5984645f4e94c.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_data.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_data'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_DATA_H__
-#define __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_DATA_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-extern boolean_T method_not_empty;
-extern uint32_T state[625];
-
-/* Variable Definitions */
-
-/* Function Declarations */
-#endif
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_data.h) */
diff --git a/.svn/pristine/d0/d06b66a5b8fd707a2b548ec426b76a25043d0e68.svn-base b/.svn/pristine/d0/d06b66a5b8fd707a2b548ec426b76a25043d0e68.svn-base
deleted file mode 100644
index ba7d2e2e0d5fe7cc8f591a8388b5459efd75424f..0000000000000000000000000000000000000000
--- a/.svn/pristine/d0/d06b66a5b8fd707a2b548ec426b76a25043d0e68.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * quat2rot.h
- *
- * Code generation for function 'quat2rot'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __QUAT2ROT_H__
-#define __QUAT2ROT_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void quat2rot(const real_T Q[4], real_T R[9]);
-#endif
-/* End of code generation (quat2rot.h) */
diff --git a/.svn/pristine/d5/d59a6e21a96157b0cf9d35989921ac4d1ca15c9d.svn-base b/.svn/pristine/d5/d59a6e21a96157b0cf9d35989921ac4d1ca15c9d.svn-base
deleted file mode 100644
index c7ec30c341f45cb19be5fd066c70dd87f1793f68..0000000000000000000000000000000000000000
--- a/.svn/pristine/d5/d59a6e21a96157b0cf9d35989921ac4d1ca15c9d.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime_terminate.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime_terminate'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_TERMINATE_H__
-#define __OPTIMAL_AFFINE_TRACKING_3D16_FAST_REALTIME_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void Optimal_affine_tracking_3d16_fast_realtime_terminate(void);
-#endif
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime_terminate.h) */
diff --git a/.svn/pristine/e2/e2385d76a038ceeb0e6bca361e80981b7857c8c8.svn-base b/.svn/pristine/e2/e2385d76a038ceeb0e6bca361e80981b7857c8c8.svn-base
deleted file mode 100644
index 1dfc9f0532971acc5c1a5de8a6566685c4eaa246..0000000000000000000000000000000000000000
--- a/.svn/pristine/e2/e2385d76a038ceeb0e6bca361e80981b7857c8c8.svn-base
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- * Optimal_affine_tracking_3d16_fast_realtime.cpp
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "compute_prob1.h"
-#include "LoadKinectMesh_realtime.h"
-#include "init_variables.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void Optimal_affine_tracking_3d16_fast_realtime(const real_T pty[4], const
- real_T ptx[4], real_T p[12])
-{
- static real_T Irgb[504063];
- static real_T Ixyz[504063];
- real_T center_y;
- real_T center_x;
- real_T corner_p[12];
- static real_T mean_img[3888];
- static real_T point_matrix[3888];
- static real_T AR_velocity[2250000];
- static real_T X_par[2250000];
- static real_T dw_dp[15552];
- static real_T tracked_images[38880000];
- static real_T X_par_pred[2250000];
- int32_T t;
- real_T centroid[3];
- real_T Aff_matrix[9];
- int32_T i0;
- int32_T i1;
- int32_T i2;
-
- /* % Object template initialization */
- /* [Ixyz,Irgb] = LoadKinectMeshBinary_realtime; */
- LoadKinectMesh_realtime(Ixyz, Irgb);
-
- /* h1=figure(1); */
- init_variables(Ixyz, ptx, pty, X_par_pred, tracked_images, dw_dp, X_par,
- AR_velocity, point_matrix, mean_img, corner_p, ¢er_x,
- ¢er_y);
-
- /* % Execute tracking */
- for (t = 0; t < 9999; t++) {
- LoadKinectMesh_realtime(Ixyz, Irgb);
-
- /* [Ixyz,Irgb] = LoadKinectMeshBinary_realtime; */
- /* %% Particle propagation and likelihood computation */
- compute_prob1(X_par, X_par_pred, AR_velocity, dw_dp, (real_T)t + 2.0,
- center_x, center_y, Ixyz, point_matrix, mean_img,
- tracked_images, Aff_matrix, centroid);
-
- /* %% Display the tracking results */
- /* set(0,'CurrentFigure',h1); */
- /* imshow(Irgb/255); */
- for (i0 = 0; i0 < 3; i0++) {
- for (i1 = 0; i1 < 4; i1++) {
- p[i0 + 3 * i1] = 0.0;
- for (i2 = 0; i2 < 3; i2++) {
- p[i0 + 3 * i1] += Aff_matrix[i0 + 3 * i2] * corner_p[i2 + 3 * i1];
- }
- }
- }
-
- /* hold on;line([p(2,1) p(2,2)],[p(1,1) p(1,2)],'Color','red','LineWidth',3); */
- /* hold on;line([p(2,2) p(2,3)],[p(1,2) p(1,3)],'Color','red','LineWidth',3); */
- /* hold on;line([p(2,3) p(2,4)],[p(1,3) p(1,4)],'Color','red','LineWidth',3); */
- /* hold on;line([p(2,4) p(2,1)],[p(1,4) p(1,1)],'Color','red','LineWidth',3); */
- /* hold off; */
- /* drawnow; */
- }
-}
-
-/* End of code generation (Optimal_affine_tracking_3d16_fast_realtime.cpp) */
diff --git a/.svn/pristine/e6/e6c33b8d780a5a965deaeeab4a59b5e8d55f8305.svn-base b/.svn/pristine/e6/e6c33b8d780a5a965deaeeab4a59b5e8d55f8305.svn-base
deleted file mode 100644
index c8267805ec43287a34bf1f948667168c61422efd..0000000000000000000000000000000000000000
--- a/.svn/pristine/e6/e6c33b8d780a5a965deaeeab4a59b5e8d55f8305.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * estimateRigidTransform.h
- *
- * Code generation for function 'estimateRigidTransform'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __ESTIMATERIGIDTRANSFORM_H__
-#define __ESTIMATERIGIDTRANSFORM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void estimateRigidTransform(const real_T x[3888], const real_T y[3888], real_T T[16]);
-#endif
-/* End of code generation (estimateRigidTransform.h) */
diff --git a/.svn/pristine/ed/ed5fb69ba2004c49e52a8a6b447c8ca453284671.svn-base b/.svn/pristine/ed/ed5fb69ba2004c49e52a8a6b447c8ca453284671.svn-base
deleted file mode 100644
index a1012b80552985fd62506f59775d591a5f44fa33..0000000000000000000000000000000000000000
--- a/.svn/pristine/ed/ed5fb69ba2004c49e52a8a6b447c8ca453284671.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * eye.h
- *
- * Code generation for function 'eye'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __EYE_H__
-#define __EYE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_eye(real_T I[9]);
-extern void eye(real_T I[9]);
-#endif
-/* End of code generation (eye.h) */
diff --git a/.svn/pristine/f1/f117f500aad723afe8ec0809b0525b9fd533cbd3.svn-base b/.svn/pristine/f1/f117f500aad723afe8ec0809b0525b9fd533cbd3.svn-base
deleted file mode 100644
index 5d12212d280c433a743334282fb2d5ef2039a2e4..0000000000000000000000000000000000000000
--- a/.svn/pristine/f1/f117f500aad723afe8ec0809b0525b9fd533cbd3.svn-base
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * norm.cpp
- *
- * Code generation for function 'norm'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "norm.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-real_T b_norm(const real_T x[3888])
-{
- real_T y;
- int32_T k;
- y = 0.0;
- for (k = 0; k < 3888; k++) {
- y += fabs(x[k]);
- }
-
- return y;
-}
-
-real_T norm(const real_T x[1296])
-{
- real_T y;
- int32_T k;
- y = 0.0;
- for (k = 0; k < 1296; k++) {
- y += fabs(x[k]);
- }
-
- return y;
-}
-
-/* End of code generation (norm.cpp) */
diff --git a/.svn/pristine/f5/f5a7430dae8d42501bfe05222265d670ad04c8dd.svn-base b/.svn/pristine/f5/f5a7430dae8d42501bfe05222265d670ad04c8dd.svn-base
deleted file mode 100644
index 24ffeb00799dfa4c5c9614a44885352d966fd27f..0000000000000000000000000000000000000000
--- a/.svn/pristine/f5/f5a7430dae8d42501bfe05222265d670ad04c8dd.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * init_variables.h
- *
- * Code generation for function 'init_variables'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __INIT_VARIABLES_H__
-#define __INIT_VARIABLES_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void init_variables(const real_T Ixyz[504063], const real_T ptx[4], const real_T pty[4], real_T X_par_pred[2250000], real_T tracked_images[38880000], real_T dw_dp[15552], real_T X_par[2250000], real_T AR_velocity[2250000], real_T point_matrix[3888], real_T mean_img[3888], real_T corner_p[12], real_T *center_x, real_T *center_y);
-#endif
-/* End of code generation (init_variables.h) */
diff --git a/.svn/pristine/f5/f5e4af1ee68daa6ba75d1146fbdb0248e76d50ee.svn-base b/.svn/pristine/f5/f5e4af1ee68daa6ba75d1146fbdb0248e76d50ee.svn-base
deleted file mode 100644
index d9cde68d3fe2ececd472cf81b898628c7eafae0c..0000000000000000000000000000000000000000
--- a/.svn/pristine/f5/f5e4af1ee68daa6ba75d1146fbdb0248e76d50ee.svn-base
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * rt_defines.h
- *
- * Code generation for function 'Optimal_affine_tracking_3d16_fast_realtime'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RT_DEFINES_H__
-#define __RT_DEFINES_H__
-
-#include <stdlib.h>
-
-#define RT_PI 3.14159265358979323846
-#define RT_PIF 3.1415927F
-#define RT_LN_10 2.30258509299404568402
-#define RT_LN_10F 2.3025851F
-#define RT_LOG10E 0.43429448190325182765
-#define RT_LOG10EF 0.43429449F
-#define RT_E 2.7182818284590452354
-#define RT_EF 2.7182817F
-#endif
-/* End of code generation (rt_defines.h) */
diff --git a/.svn/pristine/f6/f67d3f0b82fec8ab917c782c92742dbf1789f6ed.svn-base b/.svn/pristine/f6/f67d3f0b82fec8ab917c782c92742dbf1789f6ed.svn-base
deleted file mode 100644
index cdbde1c3dd3fbc1481488c5aee5a5b8ec1664e3b..0000000000000000000000000000000000000000
--- a/.svn/pristine/f6/f67d3f0b82fec8ab917c782c92742dbf1789f6ed.svn-base
+++ /dev/null
@@ -1,365 +0,0 @@
-/*
- * randn.cpp
- *
- * Code generation for function 'randn'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime.h"
-#include "randn.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real_T eml_rand_mt19937ar(uint32_T b_state[625]);
-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);
-
-/* Function Definitions */
-static real_T eml_rand_mt19937ar(uint32_T b_state[625])
-{
- real_T r;
- int32_T exitg1;
- uint32_T u32[2];
- int32_T i;
- static const real_T dv15[257] = { 0.0, 0.215241895984875, 0.286174591792068,
- 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
- 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
- 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
- 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
- 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
- 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
- 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
- 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
- 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
- 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
- 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
- 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
- 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
- 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
- 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
- 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
- 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
- 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
- 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
- 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
- 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
- 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
- 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
- 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
- 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
- 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
- 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
- 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
- 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
- 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
- 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
- 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
- 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
- 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
- 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
- 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
- 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
- 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
- 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
- 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
- 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
- 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
- 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
- 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
- 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
- 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
- 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
- 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
- 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
- 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
- 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
- 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
- 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
- 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
- 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
- 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
- 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
- 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
- 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
- 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
- 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
- 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
- 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
- 3.65415288536101, 3.91075795952492 };
-
- real_T u;
- static const real_T dv16[257] = { 1.0, 0.977101701267673, 0.959879091800108,
- 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
- 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
- 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
- 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
- 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
- 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
- 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
- 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
- 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
- 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
- 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
- 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
- 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
- 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
- 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
- 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
- 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
- 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
- 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
- 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
- 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
- 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
- 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
- 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
- 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
- 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
- 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
- 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
- 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
- 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
- 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
- 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
- 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
- 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
- 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
- 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
- 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
- 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
- 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
- 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
- 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
- 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
- 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
- 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
- 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
- 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
- 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
- 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
- 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
- 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
- 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
- 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
- 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
- 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
- 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
- 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
- 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
- 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
- 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
- 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
- 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
- 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
- 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
- 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
- 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
- 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
- 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
- 0.000477467764609386 };
-
- real_T x;
- do {
- exitg1 = 0;
- genrand_uint32_vector(b_state, u32);
- i = (int32_T)((u32[1] >> 24U) + 1U);
- r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
- 16777215)) * 2.2204460492503131E-16 - 1.0) * dv15[i];
- if (fabs(r) <= dv15[i - 1]) {
- exitg1 = 1;
- } else if (i < 256) {
- u = genrandu(b_state);
- if (dv16[i] + u * (dv16[i - 1] - dv16[i]) < exp(-0.5 * r * r)) {
- exitg1 = 1;
- }
- } else {
- do {
- u = genrandu(b_state);
- x = log(u) * 0.273661237329758;
- u = genrandu(b_state);
- } while (!(-2.0 * log(u) > x * x));
-
- if (r < 0.0) {
- r = x - 3.65415288536101;
- } else {
- r = 3.65415288536101 - x;
- }
-
- exitg1 = 1;
- }
- } while (exitg1 == 0);
-
- return r;
-}
-
-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])
-{
- int32_T i;
- uint32_T mti;
- int32_T kk;
- uint32_T y;
- uint32_T b_y;
- uint32_T c_y;
- uint32_T d_y;
- for (i = 0; i < 2; i++) {
- u[i] = 0U;
- }
-
- for (i = 0; i < 2; i++) {
- mti = mt[624] + 1U;
- if (mti >= 625U) {
- for (kk = 0; kk < 227; kk++) {
- y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- b_y = y >> 1U;
- } else {
- b_y = y >> 1U ^ 2567483615U;
- }
-
- mt[kk] = mt[397 + kk] ^ b_y;
- }
-
- for (kk = 0; kk < 396; kk++) {
- y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- c_y = y >> 1U;
- } else {
- c_y = y >> 1U ^ 2567483615U;
- }
-
- mt[227 + kk] = mt[kk] ^ c_y;
- }
-
- y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- d_y = y >> 1U;
- } else {
- d_y = y >> 1U ^ 2567483615U;
- }
-
- mt[623] = mt[396] ^ d_y;
- mti = 1U;
- }
-
- y = mt[(int32_T)mti - 1];
- mt[624] = mti;
- y ^= y >> 11U;
- y ^= y << 7U & 2636928640U;
- y ^= y << 15U & 4022730752U;
- y ^= y >> 18U;
- u[i] = y;
- }
-}
-
-real_T genrandu(uint32_T mt[625])
-{
- real_T r;
- int32_T exitg1;
- uint32_T u[2];
- boolean_T isvalid;
- int32_T k;
- boolean_T exitg2;
- uint32_T b_r;
-
- /* <LEGAL> This is a uniform (0,1) pseudorandom number generator based on: */
- /* <LEGAL> */
- /* <LEGAL> A C-program for MT19937, with initialization improved 2002/1/26. */
- /* <LEGAL> Coded by Takuji Nishimura and Makoto Matsumoto. */
- /* <LEGAL> */
- /* <LEGAL> Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */
- /* <LEGAL> All rights reserved. */
- /* <LEGAL> */
- /* <LEGAL> Redistribution and use in source and binary forms, with or without */
- /* <LEGAL> modification, are permitted provided that the following conditions */
- /* <LEGAL> are met: */
- /* <LEGAL> */
- /* <LEGAL> 1. Redistributions of source code must retain the above copyright */
- /* <LEGAL> notice, this list of conditions and the following disclaimer. */
- /* <LEGAL> */
- /* <LEGAL> 2. Redistributions in binary form must reproduce the above copyright */
- /* <LEGAL> notice, this list of conditions and the following disclaimer in the */
- /* <LEGAL> documentation and/or other materials provided with the distribution. */
- /* <LEGAL> */
- /* <LEGAL> 3. The names of its contributors may not be used to endorse or promote */
- /* <LEGAL> products derived from this software without specific prior written */
- /* <LEGAL> permission. */
- /* <LEGAL> */
- /* <LEGAL> THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
- /* <LEGAL> "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
- /* <LEGAL> LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */
- /* <LEGAL> A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */
- /* <LEGAL> CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */
- /* <LEGAL> EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */
- /* <LEGAL> PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */
- /* <LEGAL> PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */
- /* <LEGAL> LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */
- /* <LEGAL> NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
- /* <LEGAL> SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
- do {
- exitg1 = 0;
- genrand_uint32_vector(mt, u);
- r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)
- (u[1] >> 6U));
- if (r == 0.0) {
- if ((mt[624] >= 1U) && (mt[624] < 625U)) {
- isvalid = TRUE;
- } else {
- isvalid = FALSE;
- }
-
- if (isvalid) {
- isvalid = FALSE;
- k = 1;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (k < 625)) {
- if (mt[k - 1] == 0U) {
- k++;
- } else {
- isvalid = TRUE;
- exitg2 = TRUE;
- }
- }
- }
-
- if (!isvalid) {
- b_r = 5489U;
- mt[0] = 5489U;
- for (k = 0; k < 623; k++) {
- b_r = (b_r ^ b_r >> 30U) * 1812433253U + (uint32_T)(1 + k);
- mt[1 + k] = b_r;
- }
-
- mt[624] = 624U;
- }
- } else {
- exitg1 = 1;
- }
- } while (exitg1 == 0);
-
- return r;
-}
-
-void randn(real_T r[6])
-{
- int32_T k;
- if (!method_not_empty) {
- method_not_empty = TRUE;
- }
-
- for (k = 0; k < 6; k++) {
- r[k] = eml_rand_mt19937ar(state);
- }
-}
-
-/* End of code generation (randn.cpp) */
diff --git a/.svn/pristine/f7/f7c6eb80da9985b30f2576ae017162e10ad557a9.svn-base b/.svn/pristine/f7/f7c6eb80da9985b30f2576ae017162e10ad557a9.svn-base
deleted file mode 100644
index 0eeb67f7651d0434847eed196b915dde8c4706ca..0000000000000000000000000000000000000000
--- a/.svn/pristine/f7/f7c6eb80da9985b30f2576ae017162e10ad557a9.svn-base
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * resampling.h
- *
- * Code generation for function 'resampling'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __RESAMPLING_H__
-#define __RESAMPLING_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void resampling(const real_T w[25], real_T outindex[25]);
-#endif
-/* End of code generation (resampling.h) */
diff --git a/.svn/pristine/f8/f81e0064dd0a10535e860616b85eb210fd25436b.svn-base b/.svn/pristine/f8/f81e0064dd0a10535e860616b85eb210fd25436b.svn-base
deleted file mode 100644
index 9f1c0aa02a0d421befabc04665066ccb7dea1277..0000000000000000000000000000000000000000
--- a/.svn/pristine/f8/f81e0064dd0a10535e860616b85eb210fd25436b.svn-base
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * image_warping.h
- *
- * Code generation for function 'image_warping'
- *
- * C source code generated on: Tue Apr 1 12:30:12 2014
- *
- */
-
-#ifndef __IMAGE_WARPING_H__
-#define __IMAGE_WARPING_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "Optimal_affine_tracking_3d16_fast_realtime_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_image_warping(const real_T frame[336042], const real_T aff_matrix[9], const real_T point_matrix[3888], real_T warped_img[1296]);
-extern void image_warping(const real_T frame[168021], const real_T aff_matrix[9], const real_T point_matrix[3888], real_T warped_img[1296]);
-#endif
-/* End of code generation (image_warping.h) */
diff --git a/.svn/pristine/f9/f9939da1daa68dd7a9085993cacf814f0eb6af8c.svn-base b/.svn/pristine/f9/f9939da1daa68dd7a9085993cacf814f0eb6af8c.svn-base
deleted file mode 100644
index 31bfcfef49f20041e2f6b32d812f60d093632bd1..0000000000000000000000000000000000000000
--- a/.svn/pristine/f9/f9939da1daa68dd7a9085993cacf814f0eb6af8c.svn-base
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b iri_depth_tracker is ...
-
-<!--
-Provide an overview of your package.
--->
-
-
-\section codeapi Code API
-
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
-
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
-
-*/
diff --git a/.svn/wc.db b/.svn/wc.db
deleted file mode 100644
index 76b5df070f81f440ff9059c87619416cafbf3790..0000000000000000000000000000000000000000
Binary files a/.svn/wc.db and /dev/null differ