void ThreadProc( PBYTE *pMem )
{
int i;
InterlockedIncrement( (PLONG)&nThreads );
Sleep(500); // wait for more threads to start up too..
// initial state - have to do an allocate....
// Release( Allocate( 1 ) );
// DebugDumpMem();
for( i = 0; i < 10; i++ )
{
T1 (A1(pMem));
// Sleep(0);
T2 (A1(pMem));
// Sleep(0);
T3 (A1(pMem));
// Sleep(0);
T3i (A1(pMem));
// Sleep(0);
T3is(A1(pMem));
// Sleep(0);
T4 (A1(pMem));
// Sleep(0);
T5 (A1(pMem));
// Sleep(0);
T1 (A2(pMem));
// Sleep(0);
T2 (A2(pMem));
// Sleep(0);
T3 (A2(pMem));
// Sleep(0);
T3i (A2(pMem));
// Sleep(0);
T3is(A2(pMem));
// Sleep(0);
T4 (A2(pMem));
// Sleep(0);
T5 (A1(pMem));
// Sleep(0);
T1 (A2i(pMem));
// Sleep(0);
T2 (A2i(pMem));
// Sleep(0);
T3 (A2i(pMem));
// Sleep(0);
T3i (A2i(pMem));
// Sleep(0);
T3is(A2i(pMem));
// Sleep(0);
T4 (A2i(pMem));
// Sleep(0);
T5 (A1(pMem));
// Sleep(0);
}
InterlockedDecrement( (PLONG)&nThreads );
ExitThread(0);
}
typename ::std::enable_if <
::std::is_same<float, decltype(T1()+T2()+T3())>::value,
decltype(T1()+T2()+T3())
>::type
inline fmac(T1 x, T2 y, T3 z) {
#ifdef FP_FAST_FMAF
return ::std::fma(x,y,z);
#else
return x*y+z;
#endif
}
sequence(
const T1& t1 = T1(),
const T2& t2 = T2(),
const T3& t3 = T3(),
const T4& t4 = T4(),
const T5& t5 = T5(),
const T6& t6 = T6(),
const T7& t7 = T7(),
const T8& t8 = T8(),
const T9& t9 = T9(),
const T10& t10 = T10(),
const T11& t11 = T11(),
const T12& t12 = T12(),
const T13& t13 = T13(),
const T14& t14 = T14(),
const T15& t15 = T15(),
const T16& t16 = T16(),
const T17& t17 = T17() ) :
p1(t1),
p2(t2),
p3(t3),
p4(t4),
p5(t5),
p6(t6),
p7(t7),
p8(t8),
p9(t9),
p10(t10),
p11(t11),
p12(t12),
p13(t13),
p14(t14),
p15(t15),
p16(t16),
p17(t17) {}
Pose6d Pose6d::operator * ( Pose6d pose2) const
{
tf::Matrix3x3 R1 = getBasis();
tf::Matrix3x3 R2 = pose2.getBasis();
tf::Vector3 T1 = getOrigin();
tf::Vector3 T2 = pose2.getOrigin();
tf::Matrix3x3 R3;
R3[0][0] = R1[0][0] * R2[0][0] + R1[0][1]*R2[1][0] + R1[0][2]*R2[2][0];
R3[1][0] = R1[1][0] * R2[0][0] + R1[1][1]*R2[1][0] + R1[1][2]*R2[2][0];
R3[2][0] = R1[2][0] * R2[0][0] + R1[2][1]*R2[1][0] + R1[2][2]*R2[2][0];
R3[0][1] = R1[0][0] * R2[0][1] + R1[0][1]*R2[1][1] + R1[0][2]*R2[2][1];
R3[1][1] = R1[1][0] * R2[0][1] + R1[1][1]*R2[1][1] + R1[1][2]*R2[2][1];
R3[2][1] = R1[2][0] * R2[0][1] + R1[2][1]*R2[1][1] + R1[2][2]*R2[2][1];
R3[0][2] = R1[0][0] * R2[0][2] + R1[0][1]*R2[1][2] + R1[0][2]*R2[2][2];
R3[1][2] = R1[1][0] * R2[0][2] + R1[1][1]*R2[1][2] + R1[1][2]*R2[2][2];
R3[2][2] = R1[2][0] * R2[0][2] + R1[2][1]*R2[1][2] + R1[2][2]*R2[2][2];
double tempx, tempy, tempz;
tempx = R1[0][0] * T2.x() + R1[0][1]*T2.y() + R1[0][2]*T2.z() + T1.x();
tempy = R1[1][0] * T2.x() + R1[1][1]*T2.y() + R1[1][2]*T2.z() + T1.y();
tempz = R1[2][0] * T2.x() + R1[2][1]*T2.y() + R1[2][2]*T2.z() + T1.z();
tf::Vector3 T3(tempx, tempy, tempz);
Pose6d pose;
pose.setBasis(R3);
pose.setOrigin(T3);
return(pose);
}
double getAverage(T3 array[],int n){
T3 sum = T3(); //sum = 0
for(int i=0;i<n;i++)
sum += array[i];
return double(sum)/n;
}
int main()
{
T1();
T2();
T3();
T4();
return 0;
}
// calculate gfield close to the nth caustic ring flow
// two cases: within the caustic, all four roots are real, so we use T1 for d4 and T2,3,4 for d3
// outside the caustic there should be two real roots and two complex roots
// the vector intlimits hold the limits of integration; for the first case, intlimits is of length four; second, length two
void gfield_close(double rho, double z, int n, double *rfield, double *zfield) {
z = (double complex)(z / p_n[n]);
double complex x = (double complex)(( rho - a_n[n] ) / p_n[n]);
double im[4] = {cabs(cimag(T1(x,z))), cabs(cimag(T2(x,z))), cabs(cimag(T3(x,z))), cabs(cimag(T4(x,z)))};
double re[4] = {creal(T1(x,z)) , creal(T2(x,z)) , creal(T3(x,z)) , creal(T4(x,z))};
int count = 0;
int i, j;
double temp;
// get only the roots we want
for (i = 0; i < 4; ++i) {
if (im[i] < TOLERANCE) {
re[count] = re[i];
++count; // count the roots - there should be 2 or 4
}
}
// sort all 2 or 4 roots in ascending order
for (i = 0; i < count - 1; ++i) {
for (j = i + 1; j < count; ++j)
if (re[i] > re[j]) {
temp = re[i];
re[i] = re[j];
re[j] = temp;
}
}
// roots stored in re[]
double factor = -8.0 * M_PI * G * rate_n[n] * 4498.6589 / (rho * V_n[n] * 1.0226831);
*rfield += factor * ( creal(f5(x,z,re[1])) - creal(f5(x,z,re[0])) - 0.5 ); // it seems that the answer is just off by 0.5, so we subtract it by hand
*zfield += factor * ( cimag(f5(x,z,re[1])) - cimag(f5(x,z,re[0])) );
if (count == 4){
*rfield += factor * ( creal(f5(x,z,re[3])) - creal(f5(x,z,re[2])) );
*zfield += factor * ( cimag(f5(x,z,re[3])) - cimag(f5(x,z,re[2])) );
}
}
int main(void)
{
T1();
T2();
T3();
T4();
T5();
T6();
return 0;
}
SECStatus
rijndael_encryptBlock(AESContext *cx,
unsigned char *output,
const unsigned char *input)
{
return SECFailure;
#ifdef rijndael_large_blocks_fixed
unsigned int j, r, Nb;
unsigned int c2=0, c3=0;
PRUint32 *roundkeyw;
PRUint8 clone[RIJNDAEL_MAX_STATE_SIZE];
Nb = cx->Nb;
roundkeyw = cx->expandedKey;
/* Step 1: Add Round Key 0 to initial state */
for (j=0; j<4*Nb; j+=4) {
COLUMN(clone, j) = COLUMN(input, j) ^ *roundkeyw++;
}
/* Step 2: Loop over rounds [1..NR-1] */
for (r=1; r<cx->Nr; ++r) {
for (j=0; j<Nb; ++j) {
COLUMN(output, j) = T0(STATE_BYTE(4* j )) ^
T1(STATE_BYTE(4*((j+ 1)%Nb)+1)) ^
T2(STATE_BYTE(4*((j+c2)%Nb)+2)) ^
T3(STATE_BYTE(4*((j+c3)%Nb)+3));
}
for (j=0; j<4*Nb; j+=4) {
COLUMN(clone, j) = COLUMN(output, j) ^ *roundkeyw++;
}
}
/* Step 3: Do the last round */
/* Final round does not employ MixColumn */
for (j=0; j<Nb; ++j) {
COLUMN(output, j) = ((BYTE0WORD(T2(STATE_BYTE(4* j )))) |
(BYTE1WORD(T3(STATE_BYTE(4*(j+ 1)%Nb)+1))) |
(BYTE2WORD(T0(STATE_BYTE(4*(j+c2)%Nb)+2))) |
(BYTE3WORD(T1(STATE_BYTE(4*(j+c3)%Nb)+3)))) ^
*roundkeyw++;
}
return SECSuccess;
#endif
}
void operate(array<T1, S1>& u, array <T2, S2>& p, array <T3, S3>& q, sep,
const array<T4, S4>& a, const size_array& idx = size_array()) // idx only used internally!
{
if (a.empty()) { u.init(); p.init(); q.init(); return; }
q = arr(true).cat(diff(a) != 0);
if (FIRST) (_, p) = find++(q);
else (_, p) = find++(cshift(q, -1));
u = a[p];
if (!idx.empty()) force(p) = idx[p];
q[0] = T3();
q = cumsum(q);
}
/*!
SLMesh::preShade calculates the rest of the intersection information
after the final hit point is determined. Should be called just before the
shading when the final intersection point of the closest triangle was found.
*/
void SLMesh::preShade(SLRay* ray)
{
SLFace* hit = ray->hitTriangle;
// calculate the hit point in world space
ray->hitPoint.set(ray->origin + ray->length * ray->dir);
// calculate the interpolated normal with vertex normals in object space
ray->hitNormal.set(N[hit->iA] * (1-(ray->hitU+ray->hitV)) +
N[hit->iB] * ray->hitU +
N[hit->iC] * ray->hitV);
// transform normal back to world space
ray->hitNormal.set(ray->hitShape->wmN() * ray->hitNormal);
// invert normal if the ray is inside a shape
if (!ray->isOutside) ray->hitNormal *= -1;
// for shading the normal is expected to be unit length
ray->hitNormal.normalize();
// calculate interpolated texture coordinates
SLVGLTexture& textures = ray->hitMat->textures();
if (textures.size() > 0)
{ SLVec2f Tu(Tc[hit->iB] - Tc[hit->iA]);
SLVec2f Tv(Tc[hit->iC] - Tc[hit->iA]);
SLVec2f tc(Tc[hit->iA] + ray->hitU*Tu + ray->hitV*Tv);
ray->hitTexCol.set(textures[0]->getTexelf(tc.x,tc.y));
// bumpmapping
if (textures.size() > 1)
{ if (T)
{
// calculate the interpolated tangent with vertex tangent in object space
SLVec4f hitT(T[hit->iA] * (1-(ray->hitU+ray->hitV)) +
T[hit->iB] * ray->hitU +
T[hit->iC] * ray->hitV);
SLVec3f T3(hitT.x,hitT.y,hitT.z); // tangent with 3 components
T3.set(ray->hitShape->wmN() * T3); // transform tangent back to world space
SLVec2f d = textures[1]->dsdt(tc.x,tc.y); // slope of bumpmap at tc
SLVec3f N = ray->hitNormal; // unperturbated normal
SLVec3f B(N^T3); // binormal tangent B
B *= T[hit->iA].w; // correct handedness
SLVec3f D(d.x*T3 + d.y*B); // perturbation vector D
N+=D;
N.normalize();
ray->hitNormal.set(N);
}
}
}
}
void operate(array<T1, S1>& u, array <array <T2, S21>, S22>& p,
array <T3, S3>& q, sep,
const array<T4, S4>& a, const size_array& idx = size_array()) // idx only used internally!
{
if (a.empty()) { u.init(); p.init(); q.init(); return; }
q = arr(true).cat(diff(a) != 0);
size_array first = find(q);
u = a[first];
if (idx.empty()) (_, p) = partition++(size_array((0, _, a.length() - 1)), first); // TODO: remove size_array copy
else (_, p) = partition++(idx, first);
q[0] = T3();
q = cumsum(q);
}
int main(void)
{
T1();
T2();
T3();
T4();
T5();
T6();
T7();
T8();
T9();
T10();
T11();
return 0;
}
int test3() {
tracktempo T(1.0);
if (T.getTickTime(5) != 5.0) { FAIL; return 1; }
if (T.readTempoMark(0) != 1.0) { FAIL; return 1; }
T.addTempoMark(0, 2.0);
if (T.getTickTime(5) != 10.0) { FAIL; return 1; }
if (T.readTempoMark(0) != 2.0) { FAIL; return 1; }
tracktempo T2 = T;
if (T2.getTickTime(5) != 10.0) { FAIL; return 1; }
if (T2.readTempoMark(0) != 2.0) { FAIL; return 1; }
tracktempo T3(T);
if (T3.getTickTime(5) != 10.0) { FAIL; return 1; }
if (T3.readTempoMark(0) != 2.0) { FAIL; return 1; }
return 0;
}
rgpprob1::rgpprob1() : rgp_base(NUM_VARS)
{
// Objective function: h^-1 w^-1 d^-1 (inverse of volume)
{ monomial<aaf> obj(NUM_VARS);
obj._a[h] = aaf(-1.0); obj._a[w] = aaf(-1.0); obj._a[d] = aaf(-1.0);
obj.set_coeff(aaf(1.0));
rgp_base::_M.push_back( posynomial<aaf>(obj) ); }
// (2/Awall)hw + (2/Awall)hd <= 1
{ monomial<aaf> T11(NUM_VARS);
T11._a[h] = aaf(1.0); T11._a[w] = aaf(1.0);
T11.set_coeff(2./Awall);
monomial<aaf> T12(NUM_VARS);
T12._a[h] = aaf(1.0); T12._a[d] = aaf(1.0);
T12.set_coeff(2./Awall);
posynomial<aaf> P1(T11);
P1 += T12;
rgp_base::_M.push_back(P1); }
{ monomial<aaf> T2(NUM_VARS);
T2._a[w] = aaf(1.0); T2._a[d] = aaf(1.0);
T2.set_coeff(1./Aflr);
rgp_base::_M.push_back( posynomial<aaf>(T2) ); }
{ monomial<aaf> T3(NUM_VARS);
T3._a[h] = aaf(-1.0); T3._a[w] = aaf(1.0);
T3.set_coeff(alpha);
rgp_base::_M.push_back( posynomial<aaf>(T3) ); }
{ monomial<aaf> T4(NUM_VARS);
T4._a[h] = aaf(1.0); T4._a[w] = aaf(-1.0);
T4.set_coeff(1./beta);
rgp_base::_M.push_back( posynomial<aaf>(T4) ); }
{ monomial<aaf> T5(NUM_VARS);
T5._a[w] = aaf(1.0); T5._a[d] = aaf(-1.0);
T5.set_coeff(gamma2);
rgp_base::_M.push_back( posynomial<aaf>(T5) ); }
{ monomial<aaf> T6(NUM_VARS);
T6._a[w] = aaf(-1.0); T6._a[d] = aaf(1.0);
T6.set_coeff(1./delta);
rgp_base::_M.push_back( posynomial<aaf>(T6) ); }
}
Sphere::Sphere(const Point & pa,
const Point & pb,
const Point & pc,
const Point & pd)
{
Point pad = pa - pd;
Point pbd = pb - pd;
Point pcd = pc - pd;
TensorValue<Real> T(pad,pbd,pcd);
Real D = T.det();
// The points had better not be coplanar
libmesh_assert_greater (std::abs(D), 1e-12);
Real e = 0.5*(pa.norm_sq() - pd.norm_sq());
Real f = 0.5*(pb.norm_sq() - pd.norm_sq());
Real g = 0.5*(pc.norm_sq() - pd.norm_sq());
TensorValue<Real> T1(e,pad(1),pad(2),
f,pbd(1),pbd(2),
g,pcd(1),pcd(2));
Real sx = T1.det()/D;
TensorValue<Real> T2(pad(0),e,pad(2),
pbd(0),f,pbd(2),
pcd(0),g,pcd(2));
Real sy = T2.det()/D;
TensorValue<Real> T3(pad(0),pad(1),e,
pbd(0),pbd(1),f,
pcd(0),pcd(1),g);
Real sz = T3.det()/D;
Point c(sx,sy,sz);
Real r = (c-pa).norm();
this->create_from_center_radius(c,r);
}
static SECStatus
rijndael_encryptBlock128(AESContext *cx,
unsigned char *output,
const unsigned char *input)
{
unsigned int r;
PRUint32 *roundkeyw;
rijndael_state state;
PRUint32 C0, C1, C2, C3;
#if defined(NSS_X86_OR_X64)
#define pIn input
#define pOut output
#else
unsigned char *pIn, *pOut;
PRUint32 inBuf[4], outBuf[4];
if ((ptrdiff_t)input & 0x3) {
memcpy(inBuf, input, sizeof inBuf);
pIn = (unsigned char *)inBuf;
} else {
pIn = (unsigned char *)input;
}
if ((ptrdiff_t)output & 0x3) {
pOut = (unsigned char *)outBuf;
} else {
pOut = (unsigned char *)output;
}
#endif
roundkeyw = cx->expandedKey;
/* Step 1: Add Round Key 0 to initial state */
COLUMN_0(state) = *((PRUint32 *)(pIn )) ^ *roundkeyw++;
COLUMN_1(state) = *((PRUint32 *)(pIn + 4 )) ^ *roundkeyw++;
COLUMN_2(state) = *((PRUint32 *)(pIn + 8 )) ^ *roundkeyw++;
COLUMN_3(state) = *((PRUint32 *)(pIn + 12)) ^ *roundkeyw++;
/* Step 2: Loop over rounds [1..NR-1] */
for (r=1; r<cx->Nr; ++r) {
/* Do ShiftRow, ByteSub, and MixColumn all at once */
C0 = T0(STATE_BYTE(0)) ^
T1(STATE_BYTE(5)) ^
T2(STATE_BYTE(10)) ^
T3(STATE_BYTE(15));
C1 = T0(STATE_BYTE(4)) ^
T1(STATE_BYTE(9)) ^
T2(STATE_BYTE(14)) ^
T3(STATE_BYTE(3));
C2 = T0(STATE_BYTE(8)) ^
T1(STATE_BYTE(13)) ^
T2(STATE_BYTE(2)) ^
T3(STATE_BYTE(7));
C3 = T0(STATE_BYTE(12)) ^
T1(STATE_BYTE(1)) ^
T2(STATE_BYTE(6)) ^
T3(STATE_BYTE(11));
/* Round key addition */
COLUMN_0(state) = C0 ^ *roundkeyw++;
COLUMN_1(state) = C1 ^ *roundkeyw++;
COLUMN_2(state) = C2 ^ *roundkeyw++;
COLUMN_3(state) = C3 ^ *roundkeyw++;
}
/* Step 3: Do the last round */
/* Final round does not employ MixColumn */
C0 = ((BYTE0WORD(T2(STATE_BYTE(0)))) |
(BYTE1WORD(T3(STATE_BYTE(5)))) |
(BYTE2WORD(T0(STATE_BYTE(10)))) |
(BYTE3WORD(T1(STATE_BYTE(15))))) ^
*roundkeyw++;
C1 = ((BYTE0WORD(T2(STATE_BYTE(4)))) |
(BYTE1WORD(T3(STATE_BYTE(9)))) |
(BYTE2WORD(T0(STATE_BYTE(14)))) |
(BYTE3WORD(T1(STATE_BYTE(3))))) ^
*roundkeyw++;
C2 = ((BYTE0WORD(T2(STATE_BYTE(8)))) |
(BYTE1WORD(T3(STATE_BYTE(13)))) |
(BYTE2WORD(T0(STATE_BYTE(2)))) |
(BYTE3WORD(T1(STATE_BYTE(7))))) ^
*roundkeyw++;
C3 = ((BYTE0WORD(T2(STATE_BYTE(12)))) |
(BYTE1WORD(T3(STATE_BYTE(1)))) |
(BYTE2WORD(T0(STATE_BYTE(6)))) |
(BYTE3WORD(T1(STATE_BYTE(11))))) ^
*roundkeyw++;
*((PRUint32 *) pOut ) = C0;
*((PRUint32 *)(pOut + 4)) = C1;
*((PRUint32 *)(pOut + 8)) = C2;
*((PRUint32 *)(pOut + 12)) = C3;
#if defined(NSS_X86_OR_X64)
#undef pIn
#undef pOut
#else
if ((ptrdiff_t)output & 0x3) {
memcpy(output, outBuf, sizeof outBuf);
}
#endif
return SECSuccess;
}
triple(const T1& a = T1(), const T2& b = T2(), const T3& c = T3()) :
first(a), second(b), third(c) {
}
sixtuple(const T1& a = T1(), const T2& b = T2(), const T3& c = T3(),
const T4& d = T4(), const T5& e = T5(), const T6& f = T6()) :
first(a), second(b), third(c), forth(d), fifth(e), sixth(f) {
}
//----------- Begin of function LinAlg::quadratic_prog ------//
//! Interior Point Quadratic Programming
//! c, Q, A, b, xNames (no global or member variable access)
//!
//! try, by BM:
//! c = { 0,0 }
//! Q = { {2,0}, {0,5} }
//! A = { {5,6} }
//! b = { 10 }
//! xNames={x1,x2}
//!
bool LinearAlgebra::quadratic_prog(const Vector &v_c, const Matrix &m_Q, const Matrix &m_A, const Vector &v_b, Vector &v_xNames, int loopCountMultiplier) {
// init local variables
int maxIteration = 20;
const REAL SIGMA = 1/15.0; // 1/10.0;
const REAL R = 9.0/10;
int iter = 0;
int n = v_c.Storage();
int m = v_b.Storage();
maxIteration *= loopCountMultiplier;
#ifdef DEBUG_VC
DEBUG_LOG("----------- quadratic_prog begin -----------");
// print_input(c,Q,A,b,xNames)
if ( n==10 && m==12 ) {
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9), v_c(10));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f,",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12)
);
DEBUG_LOG(s);
/*sprintf(s, "%f, %f, %f, %f, %f, %f, %f, %f, %f,",
v_b(10),v_b(11),v_b(12),
v_b(13),v_b(14),v_b(15),
v_b(16),v_b(17),v_b(18)
);
DEBUG_LOG(s);
sprintf(s, "%f, %f};",
v_b(19),v_b(20));
DEBUG_LOG(s);*/
}
else if ( n==9 && m==20 ) { // stage 1
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, },",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f },",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, }, ",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20)
);
DEBUG_LOG(s);
}
else if ( n==10 && m==22 ) { // stage 2
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9),v_c(10));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, },",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f },",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, }, ",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20), v_b(21), v_b(22)
);
DEBUG_LOG(s);
}
#endif
Vector v_eN(n); v_eN = 1;
Vector v_eM(m); v_eM = 1;
Vector v_x(n); v_x = 1;
Vector v_z(n); v_z = 1;
Vector v_y(m); v_y = 1;
Vector v_w(m); v_w = 1;
// check m_A is nxm, m_Q is nxn
err_when(m_A.Nrows() != m || m_A.Ncols() != n );
err_when(m_Q.Nrows() != n || m_Q.Ncols() != n );
// init for while loop not_converged() checking
//
Vector v_Road = v_b - m_A*(v_x) + v_w;
Vector v_Sigma = v_c - m_A.t() * v_y - v_z + m_Q*v_x;
REAL gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
REAL mute = SIGMA*gamma / (n+m);
DiagonalMatrix X(n), Xinv(n);
DiagonalMatrix Y(m), Yinv(m);
DiagonalMatrix Z(n);
DiagonalMatrix W(m);
Matrix T1(n,n), T2(n,m), T3(m,n), T4(m,m);
Vector KKFvec(n+m); //, v_tmp1(n), v_tmp2(m);
Vector m_temp(m+n);
Vector v_dx(n), v_dy(m), v_dz(n), v_dw(m);
REAL theeta;
X=0; Xinv=0; Z=0;
Y=0; Yinv=0; W=0; {
// Matrix _t1(n,n), _t2(n,n); _t1=1; _t2=2;
// _t1 = SP(_t1,_t2);
}
Matrix KKFmat(m+n,m+n);
Matrix KKFmatInverse(m+n,m+n);
REAL min=1;
while ( quadratic_prog_not_converged(v_x,v_y,v_Road,v_Sigma,gamma) && iter <= maxIteration && min > 0.0000000001 ) {
iter++;
v_Road = v_b - m_A*v_x + v_w;
v_Sigma = v_c - m_A.t()*v_y - v_z + m_Q*v_x;
gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
mute = SIGMA*gamma / (n+m);
X.set_diagonal(v_x);
Y.set_diagonal(v_y);
Z.set_diagonal(v_z);
W.set_diagonal(v_w);
Xinv.set_diagonal(ElmDivide(v_eN, v_x));
Yinv.set_diagonal(ElmDivide(v_eM, v_y));
T1 = -(Xinv * Z + m_Q);
T2 = m_A.t();
T3 = m_A;
T4 = Yinv*W;
//PRT_MAT << "T1: " << T1 << endl;
KKFmat = (T1|T2) & (T3|T4);
KKFvec = (v_c - T2*v_y - mute*Xinv*v_eN + m_Q*v_x)
& (v_b - m_A*v_x + mute*Yinv*v_eM);
Try {
/*{
bool _f;
DiagonalMatrix _dmat(n+m); _dmat=1;
Matrix _inv(n+m,n+m);
SVD(KKFmat, _dmat, _inv);
_dmat=1;
if ( KKFmat*_inv == _dmat )
_f = true;
else
_f = false;
}*/
min = fabs(KKFmat(1,1));
for (int i=2; i<=m+n; i++) {
if ( min > fabs(KKFmat(i,i)) ) // check diagonal element
min = fabs(KKFmat(i,i));
}
KKFmatInverse = KKFmat.i();
/*
if ( KKFmat.LogDeterminant().Value() )
KKFmatInverse = KKFmat.i();
else
break;
*/
m_temp = KKFmatInverse * KKFvec;
v_dx = m_temp.Rows(1,n);
v_dy = m_temp.Rows(n+1,n+m);
v_dz = Xinv*(mute*v_eN - X*Z*v_eN - Z*v_dx);
v_dw = Yinv*(mute*v_eM - Y*W*v_eM - W*v_dy);
//PRT_MAT << "v_dx,z,y,w: " << (v_dx|v_dz) <<", "<< (v_dy|v_dw) <<endl;
//PRT_MAT << "v_x: " << v_x << endl;
//PRT_MAT << "ElmDivide(v_x,v_dx): " << ElmDivide(v_x,v_dx) << endl;
theeta =
(R/((
ElmDivide(-v_dx,v_x)
& ElmDivide(-v_dw,v_w)
& ElmDivide(-v_dy,v_y)
& ElmDivide(-v_dz,v_z)
)).MaximumValue()
);
if(theeta>1) // theeta = min(theeta,1)
theeta = 1;
v_x += theeta * v_dx;
v_y += theeta * v_dy;
v_w += theeta * v_dw;
v_z += theeta * v_dz;
#ifdef DEBUG_CONSOLE
cout << "#iter "<< iter << ": " << v_Road.Sum() << ", " << v_Sigma.Sum() << ", " << gamma << endl;
cout << "Ro:" << v_Road << endl;
PRT_MAT15 << "v_x:" << v_x << endl;
PRT_MAT15 << "v_y:" << v_y << endl;
PRT_MAT15 << "v_w:" << v_w << endl;
PRT_MAT15 << "v_z:" << v_z << endl;
#elif defined(DEBUG_VC)
char s[200];
sprintf(s, "#iter %d: %f, %f, %f",
iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
DEBUG_LOG(s);
if ( n == 9 && false ) {
DEBUG_LOG("x = ");
sprintf(s, " %f, %f, %f, %f, %f, %f, %f, %f, %f",
v_x(1),v_x(2),v_x(3),
v_x(4),v_x(5),v_x(6),
v_x(7),v_x(8),v_x(9));
DEBUG_LOG(s);
}
#endif
}
CatchAll {
#ifdef DEBUG_CONSOLE
cout << Exception::what();
#endif
#ifdef DEBUG_VC
DEBUG_LOG("olinalg: failure in quad_prog");
DEBUG_LOG((char *)Exception::what());
#endif
return false;
}
} // while
v_xNames = v_x;
if ( min < 0.00001 ) { // 1214 || KKFmat.LogDeterminant().Value() == 0 )
//char s[200];
//sprintf(s, "#iter %d: %f, %f, %f",
// iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
//DEBUG_LOG(s);
DEBUG_LOG("--- quad_prog early exit for min < 0.00001 ---");
}
else
DEBUG_LOG("----------- quad_prog normal exit -----------");
#ifdef DEBUG_CONSOLE
cout << "#iter: " << iter;
#endif
return true;
}
/**
* @function calculateTrifocalTensor
*/
void trifocalTensor::calculateTrifocalTensor() {
Eigen::JacobiSVD<Eigen::MatrixXf> svd( mEq, Eigen::ComputeThinU | Eigen::ComputeThinV );
Eigen::MatrixXf V = svd.matrixV();
printf("* V has %d rows and %d cols \n", V.rows(), V.cols() );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* Rank of mEq is: %d \n", lu.rank() );
//std::cout << "Columns are nullspace : " << std::endl;
//std::cout<< lu.kernel() << std::endl;
//Eigen::MatrixXf kernel = lu.kernel();
//mmEq(mPointer, ToIndex1(3,1,2)=;3 = kernel.col( kernel.cols() - 1 );
// Eigen::MatrixXf Vt = V.transpose(); mmEq(mPointer, ToIndex1(3,1,2)=;3 = Vt.col( Vt.cols() - 1 );
mT123 = V.col( V.cols() - 1 );
printf("mT123: Rows: %d cols: %d \n", mT123.rows(), mT123.cols() );
// Saving them properly
mT.resize(0);
Eigen::MatrixXf T1(3,3);
T1(0,0) = mT123(0,0); T1(0,1) = mT123(1,0); T1(0,2) = mT123(2,0);
T1(1,0) = mT123(3,0); T1(1,1) = mT123(4,0); T1(1,2) = mT123(5,0);
T1(2,0) = mT123(6,0); T1(2,1) = mT123(7,0); T1(2,2) = mT123(8,0);
mT.push_back(T1);
printf("Saved T1 \n");
Eigen::MatrixXf T2(3,3);
T2(0,0) = mT123(9,0); T2(0,1) = mT123(10,0); T2(0,2) = mT123(11,0);
T2(1,0) = mT123(12,0); T2(1,1) = mT123(13,0); T2(1,2) = mT123(14,0);
T2(2,0) = mT123(15,0); T2(2,1) = mT123(16,0); T2(2,2) = mT123(17,0);
mT.push_back(T2);
printf("Saved T2 \n");
Eigen::MatrixXf T3(3,3);
T3(0,0) = mT123(18,0); T3(0,1) = mT123(19,0); T3(0,2) = mT123(20,0);
T3(1,0) = mT123(21,0); T3(1,1) = mT123(22,0); T3(1,2) = mT123(23,0);
T3(2,0) = mT123(24,0); T3(2,1) = mT123(25,0); T3(2,2) = mT123(26,0);
mT.push_back(T3);
printf("Saved T3 \n");
// Checking
Eigen::MatrixXf res = mEq*mT123;
std::cout << "Checking mEq*T: \n"<< res.transpose() << std::endl;
// Making it with last guy = 1
// Normalizing
for( int i = 0; i < mT.size(); ++i ) {
float temp = mT[i](2,2);
for( int j = 0; j < 3; ++j ) {
for( int k = 0; k < 3; ++k ) {
float orig = mT[i](j,k);
mT[i](j,k) = orig / temp;
}
}
}
// Visualize
for( int i = 0; i < mT.size(); ++i ) {
std::cout << "T("<<i<<"): \n" << mT[i] << std::endl;
}
// Test lines
for( int i = 0; i < mLLL.size(); ++i ) {
Eigen::VectorXf A(3);
Eigen::VectorXf B(3);
Eigen::VectorXf C(3);
Eigen::VectorXf Ap(3);
A(0) = mLLL[i][0].x;
A(1) = mLLL[i][0].y;
A(2) = mLLL[i][0].z;
B(0) = mLLL[i][1].x;
B(1) = mLLL[i][1].y;
B(2) = mLLL[i][1].z;
C(0) = mLLL[i][2].x;
C(1) = mLLL[i][2].y;
C(2) = mLLL[i][2].z;
Eigen::MatrixXf r0, r1, r2;
Eigen::MatrixXf Tt;
Tt = mT[0];
r0 = ( B.transpose() )*Tt*C;
Ap(0) = r0(0,0);
Tt = mT[1];
r1 = ( B.transpose() )*Tt*C;
Ap(1) = r1(0,0);
Tt = mT[2];
r2 = ( B.transpose() )*Tt*C;
Ap(2) = r2(0,0);
// Normalize Ap
float temp = A(2) / Ap(2);
float num;
num = Ap(0)*temp; Ap(0) = num;
num = Ap(1)*temp; Ap(1) = num;
num = Ap(2)*temp; Ap(2) = num;
std::cout <<" ("<<i<<") " <<" A: " << A.transpose() << std::endl;
std::cout <<" ("<<i<<") " <<" Ap: " << Ap.transpose() << std::endl;
}
}
Triple() : first(T1()), second(T2()), third(T3()) {}
quadruple() : first(T1()), second(T2()), third(T3()), fourth(T4()) {}
