void view_turn(view *V, const double *w)
{
#if 0
double M[16], T[16], q[4], a[3], b[3], c[3], t[3];
assert(V);
/* Transform the mark and drag vectors from eye space into the world */
/* space current at the time of the mark. */
state_matrix(M, &V->mark);
mtranspose(T, M);
vnormalize(t, V->a);
vtransform(a, T, t);
vnormalize(t, w);
vtransform(b, T, t);
/* These determine the axis and angle of rotation. Find the quaternion. */
vcrs(t, a, b);
vnormalize(c, t);
qrotate(q, c, acos(vdot(a, b)));
/* Accumulate this quaternion with the view state, rotating the view. */
qmultiply(view_curr(V)->q, V->mark.q, q);
#endif
}
static void state_inverse(double *I, const state *s)
{
double T[16], A[16], R[16];
mtranslate(T, s->p);
meuler (A, s->e);
mtranspose(R, A);
mmultiply (I, T, R);
}
/*
READ GEO
*/
void get_geometry(TE*root)
{
printf("_\t_\tGEOMETRIES\t_\t_\t_\t_\n");
TE*geo = root -> FCE( "library_geometries" )->FCE("geometry");
while ( geo )
{
get_object( geo );
geo = geo -> NSE ( "geometry" );
}
//get transforms, tranform vertices
TE*node = root -> FCE( "library_visual_scenes" )->
FCE("visual_scene") -> FCE("node");;
size_t i=0;
while ( node && i < dae_geo_objects.size() )
{
dae_geo_t &d = dae_geo_objects[i];
std::string name = node->Attribute( "name" );
if( name == d.name ){
std::vector< float >mat=str_fv(node->FCE("matrix")->GetText(),16);
//transpose
float M[16];
mtranspose( M, &mat[0] );
//multiply each vertex
for( size_t k = 0; k < d.pos.size(); k+=3 ){
vec3 vp={d.pos[k],d.pos[k+1],d.pos[k+2]};
vp = mmulv( M, vp, 1.f );
d.pos[k] = vp.x;
d.pos[k+1] = vp.y;
d.pos[k+2] = vp.z;
}
for( size_t k = 0; k < d.normal.size(); k+=3 ){
vec3 vp={d.normal[k],d.normal[k+1],d.normal[k+2]};
vp = vnormal(mmulv( M, vp, 0.f));
d.normal[k] = vp.x;
d.normal[k+1] = vp.y;
d.normal[k+2] = vp.z;
}
++i;
}
node = node -> NSE( "node" );
}
printf("count = %d\n", dae_geo_objects.size());
}
void linsolve_lower(double *L,int N,double *b,double *x) {
int i,j,c1,l;
double *y,*A;
double sum,temp;
y = (double*) malloc(sizeof(double) *N);
A = (double*) malloc(sizeof(double) *N *N);
for(i = 0; i < N;++i) {
y[i] = 0.;
x[i] = 0.;
if ( L[i*N + i] == 0.) {
printf("The Matrix system does not have a unique solution");
exit(1);
}
//printf("\n B %d",ipiv[i]);
}
// Forward Substitution
y[0] = b[0]/L[0];
for(i = 1; i < N; ++i) {
sum = 0.;
c1 = i*N;
for(j = 0; j < i; ++j) {
sum += y[j] * L[c1 + j];
}
y[i] = (b[i] - sum)/L[c1+i];
}
mtranspose(L,N,N,A);
//Back Substitution
x[N - 1] = y[N - 1]/A[N * N - 1];
for (i = N - 2; i >= 0; i--) {
sum = 0.;
c1 = i*(N+1);
l=0;
for(j = i+1; j < N;j++) {
l++;
sum += A[c1 + l] * x[j];
}
x[i] = (y[i] - sum) / A[c1];
}
free(y);
free(A);
}
int as154_seas(double *inp, int N, int optmethod, int p, int d, int q, int s, int P, int D, int Q,
double *phi, double *theta, double *PHI, double *THETA, double *wmean,double *var,double *loglik,double *hess) {
int i, pq, retval, length, offset,ret;
double *b, *tf, *x,*inp2,*dx,*thess;
int *ipiv;
double maxstep;
alik_seas_object obj;
custom_function as154_min;
obj = alik_seas_init(p, d, q, s, P, D, Q, N);
inp2 = (double*)malloc(sizeof(double)* (N - s*D));
pq = obj->pq;
b = (double*)malloc(sizeof(double)* pq);
tf = (double*)malloc(sizeof(double)* pq);
thess = (double*)malloc(sizeof(double)* pq*pq);
dx = (double*)malloc(sizeof(double)* pq);
ipiv = (int*)malloc(sizeof(int)* pq);
length = N;
maxstep = 1.0;
css_seas(inp, N, optmethod, p, d, q, s, P, D, Q, phi, theta, PHI, THETA, wmean, var,loglik,hess);
/*
*/
if (D > 0) {
N = diffs(inp, N, D, s, inp2);
}
else {
for (i = 0; i < N; ++i) {
inp2[i] = inp[i];
}
}
x = (double*)malloc(sizeof(double)* (N - d));
if (d > 0) {
N = diff(inp2, N, d, x); // No need to demean x
}
else {
for (i = 0; i < N; ++i) {
x[i] = inp2[i];
}
}
obj->N = N;
offset = obj->offset;
for (i = 0; i < p; ++i) {
b[i] = phi[i];
}
for (i = 0; i < q; ++i) {
b[p + i] = -theta[i];
}
for (i = 0; i < P; ++i) {
b[p + q + i] = PHI[i];
}
for (i = 0; i < Q; ++i) {
b[p + q + P + i] = -THETA[i];
}
if (obj->M == 1) {
b[p + q + P + Q] = *wmean;
}
obj->mean = *wmean;
//mdisplay(b, 1, p + q + P + Q);
for (i = 0; i < N; ++i) {
obj->x[offset + i] = obj->x[offset + 2 * N + i] = x[i];
}
for (i = N; i < 2 * N; ++i) {
obj->x[offset + i] = 0.0;
}
//printf("\n %d %g ", pq,maxstep);
// custom_function as154_min = { fas154_seas, obj };
as154_min.funcpt = fas154_seas;
as154_min.params = obj;
retval = fminunc(&as154_min, NULL, pq, b, maxstep, optmethod, tf);
if (retval == 0) {
ret = 0;
}
else if (retval == 15) {
ret = 15;
}
else if (retval == 4) {
ret = 4;
}
else {
ret = 1;
}
for (i = 0; i < pq; ++i) {
dx[i] = 1.0;
}
hessian_fd(&as154_min, tf, pq, dx, obj->eps, hess);
mtranspose(hess, pq, pq, thess);
for (i = 0; i < pq*pq; ++i) {
thess[i] = (N - d - s*D) * 0.5 * (hess[i] + thess[i]);
}
ludecomp(thess, pq, ipiv);
minverse(thess, pq, ipiv, hess);
for (i = 0; i < p; ++i) {
phi[i] = tf[i];
}
for (i = 0; i < q; ++i) {
theta[i] = -tf[p + i];
}
for (i = 0; i < P; ++i) {
PHI[i] = tf[p + q + i];
}
for (i = 0; i < Q; ++i) {
THETA[i] = -tf[p + q + P + i];
}
if (obj->M == 1) {
*wmean = tf[p + q + Q + P];
}
else {
*wmean = 0.0;
}
*var = (obj->ssq) / (double) N;
*loglik = obj->loglik;
//printf("MEAN %g \n", mean(obj->x+N,N));
//mdisplay(obj->x + N, 1, N);
free(b);
free(tf);
free(inp2);
free(x);
free(dx);
free(thess);
free(ipiv);
free_alik_seas(obj);
return ret;
}
int as154(double *inp, int N, int optmethod, int p, int d, int q, double *phi, double *theta, double *wmean, double *var,double *resid,double *loglik,double *hess) {
int i,pq,retval,length,ret;
double *b,*tf,*x,*dx,*thess;
int *ipiv;
double maxstep;
alik_object obj;
custom_function as154_min;
x = (double*)malloc(sizeof(double)* (N - d));
length = N;
maxstep = 1.0;
obj = alik_init(p, d, q, N);
css(inp, N, optmethod, p, d, q, phi, theta, wmean, var,resid,loglik,hess);
if (d > 0) {
N = diff(inp, N, d, x); // No need to demean x
}
else {
for (i = 0; i < N; ++i) {
x[i] = inp[i];
}
}
obj->N = N;
obj->mean = *wmean;
pq = obj->pq;
b = (double*)malloc(sizeof(double)* pq);
tf = (double*)malloc(sizeof(double)* pq);
thess = (double*)malloc(sizeof(double)* pq*pq);
dx = (double*)malloc(sizeof(double)* pq);
ipiv = (int*)malloc(sizeof(int)* pq);
for (i = 0; i < p; ++i) {
b[i] = phi[i];
}
for (i = 0; i < q; ++i) {
b[p + i] = -theta[i];
}
if (obj->M == 1) {
b[p + q] = obj->mean;
}
for (i = 0; i < N; ++i) {
obj->x[i] = obj->x[2 * N + i] = x[i];
}
for (i = N; i < 2 * N; ++i) {
obj->x[i] = 0.0;
}
//20141121ÐÞ¸Ä
//custom_function as154_min = { fas154, obj };
as154_min.funcpt = fas154;
as154_min.params = obj;
retval = fminunc(&as154_min, NULL, pq, b,maxstep, optmethod, tf);
if (retval == 0) {
ret = 0;
}
else if (retval == 15) {
ret = 15;
}
else if (retval == 4) {
ret = 4;
}
else {
ret = 1;
}
for (i = 0; i < pq; ++i) {
dx[i] = 1.0;
}
hessian_fd(&as154_min, tf, pq, dx, obj->eps, hess);
mtranspose(hess, pq, pq, thess);
for (i = 0; i < pq*pq; ++i) {
thess[i] = (N-d) * 0.5 * (hess[i] + thess[i]);
}
ludecomp(thess, pq, ipiv);
minverse(thess, pq, ipiv, hess);
for (i = 0; i < p; ++i) {
phi[i] = tf[i];
}
for (i = 0; i < q; ++i) {
theta[i] = -tf[p + i];
}
if (obj->M == 1) {
*wmean = tf[p + q];
}
else {
*wmean = 0.0;
}
/*
wmean = 0.0;
for (i = 0; i < N; ++i) {
wmean += (obj->x[N + i] * obj->x[N + i]);
}*/
*var = (obj->ssq) / (double) N;
for (i = 0; i < N - d; ++i) {
resid[i] = obj->x[N + i];
}
*loglik = obj->loglik;
//printf("MEAN %g \n", mean(obj->x+N,N));
//mdisplay(obj->x + N, 1, N);
free(b);
free(tf);
free(x);
free(thess);
free(ipiv);
free(dx);
free_alik(obj);
return ret;
}
void qrfac(double *A, int M, int N, int lda, int pivot, int *ipvt, int lipvt,double *rdiag, double *acnorm,double eps) {
int i,j,jp1,k,kmax,minmn,t;
double ajnorm,epsmch,one,p05,sum,temp,zero,temp2,pmaxval;
double *AT,*wa,*wa2;
/*
* This routine is a C translation of Fortran Code by
* argonne national laboratory. minpack project. march 1980.
burton s. garbow, kenneth e. hillstrom, jorge j. more
*
* M is a positive integer input variable set to the number
of rows of a.
N is a positive integer input variable set to the number
of columns of a.
A is an M by N array. on input a contains the matrix for
which the qr factorization is to be computed. on output
the strict upper trapezoidal part of a contains the strict
upper trapezoidal part of r, and the lower trapezoidal
part of a contains a factored form of q (the non-trivial
elements of the u vectors described above).
lda is a positive integer input variable not less than m
which specifies the leading dimension of the array a.
pivot is an integer input variable. if pivot is set to 1,
then column pivoting is enforced. if pivot is set to 0,
then no column pivoting is done.
ipvt is an integer output array of length lipvt. ipvt
defines the permutation matrix p such that a*p = q*r.
column j of p is column ipvt(j) of the identity matrix.
if pivot is false, ipvt is not referenced.
lipvt is a positive integer input variable. if pivot is false,
then lipvt may be as small as 1. if pivot is true, then
lipvt must be at least n.
rdiag is an output array of length N which contains the
diagonal elements of r.
acnorm is an output array of length N which contains the
norms of the corresponding columns of the input matrix a.
if this information is not needed, then acnorm can coincide
with rdiag.
*
*/
if (pivot != 0 && pivot != 1) {
printf("Pivot only takes binary values 0 and 1 \n");
exit(-1);
}
AT = (double*) malloc(sizeof(double) *N*M);
wa = (double*) malloc(sizeof(double) *N);
wa2 = (double*) malloc(sizeof(double) *M);
one = 1.0;
zero = 0.0;
p05 = 5.0e-02;
epsmch = eps;
mtranspose(A,M,N,AT);// AT is size NXM
//compute the initial column norms and initialize several arrays.
for(j = 0; j < N; ++j) {
acnorm[j] = enorm(AT+j*M,M);
rdiag[j] = acnorm[j];
wa[j] = rdiag[j];
if (pivot == 1) {
ipvt[j] = j;
}
}
//reduce a to r with householder transformations.
if (M < N) {
minmn = M;
} else {
minmn = N;
}
for (j = 0; j < minmn; ++j) {
if (pivot == 1) {
//bring the column of largest norm into the pivot position.
kmax = j;
for(k = j; k < N; ++k) {
if (rdiag[k] > rdiag[kmax]) {
kmax = k;
}
}
if (kmax != j) {
for(i = 0; i < M; ++i) {
t = i * N;
temp = A[t+j];
A[t+j] = A[t+kmax];
A[t+kmax] = temp;
}
rdiag[kmax] = rdiag[j];
wa[kmax] = wa[j];
k = ipvt[j];
ipvt[j] = ipvt[kmax];
ipvt[kmax] = k;
}
}
// compute the householder transformation to reduce the
// j-th column of a to a multiple of the j-th unit vector.
t = j * N + j;
for(i = 0; i < M-j; ++i) {
wa2[i] = A[t+i*N];
}
ajnorm = enorm(wa2,M-j);
if (ajnorm != zero) {
if (A[t] < zero) {
ajnorm = - ajnorm;
}
for(i = j; i < M; ++i) {
A[i*N+j] /= ajnorm;
}
A[t] += one;
// apply the transformation to the remaining columns
// and update the norms.
jp1 = j + 1; // Breakpoint
if (N >= jp1+1) {
for(k = jp1; k < N; ++k) {
sum = zero;
for(i = j; i < M; ++i) {
sum += (A[i*N+j] * A[i*N+k]);
}
temp = sum / A[t];
for(i = j; i < M; ++i) {
A[i*N+k] -= (temp * A[i*N+j]);
}
// Breakpoint 2
if (pivot == 1 && rdiag[k] != zero) {
temp = A[j*N+k] / rdiag[k];
pmaxval = pmax(zero, one - temp*temp);
rdiag[k] = rdiag[k]*sqrt(pmaxval);
temp2 = (p05*(rdiag[k]/wa[k]));
temp2 = temp2 * temp2;
if (temp2 <= epsmch) {
for(i = 0; i < M-j-1; ++i) {
wa2[i] = A[jp1*N+k+i*N];
}
rdiag[k] = enorm(wa2,M-j-1);
wa[k] = rdiag[k];
}
}
}
}
}
rdiag[j] = -ajnorm;
}
free(AT);
free(wa);
free(wa2);
}
int mesh_out( const char*name)
{
FILE*f=fopen( name , "wb");
if(!f)
return printf("File could not open: %s\n",name), -1;
{
//quick, non-optimal export
int count = v_tri.size() / 3;
std::vector<vertex_t>vertices;
std::vector<u16>indices;
for ( int i=0; i<count; ++i){
int j=i*3;
int vix = v_tri[j];
int nix = v_tri[j+1];
int tix = v_tri[j+2];
vertex_t v={
v_pos[vix*3],
v_pos[vix*3+1],
v_pos[vix*3+2],
v_uv[tix*2],
1.f-v_uv[tix*2+1],
v_normal[nix*3],
v_normal[nix*3+1],
v_normal[nix*3+2],
(float)v_jw[vix].joint_a,
(float)v_jw[vix].joint_b,
v_jw[vix].weight_a,
v_jw[vix].weight_b
};
int index = vfind( &vertices[0], v, vertices.size() );
if( index == (int)vertices.size() )
vertices.push_back(v);
indices.push_back( index );
}
//write
int num_indices = indices.size();
int num_vertices = vertices.size();
int num_bone = v_keyframes.size();
int num_frame = v_keyframes[0].size()/ 16;
u8 num_child[16]={};
u8 level[16]={};
char stack_instr[16]={};
std::vector<mat4_t>invm;
int child_count = 0;
assert( num_bone <= 16 && " TOO MANY BONES "[0] );
assert( !v_keyframes.empty() && "KEYFRAMES EMPTY"[0]);
//for( size_t i= 0 ; i < joint_hier.size(); ++i)
joint_ride(invm,num_child, joint_hier[0], level, child_count);
for(int i = 0; i<num_bone-1; ++i)
stack_instr[i]=(int)level[i+1]-(int)level[i];
//WRITE header
fwrite( &num_indices, sizeof( int ), 1, f );
fwrite( &num_vertices, sizeof( int ), 1, f );
fwrite( &num_bone, sizeof( int ) , 1, f);
fwrite( &num_frame, sizeof( int ) , 1, f);
//fwrite( num_child, sizeof(u8), 16, f);// hierarchy
fwrite( stack_instr, sizeof(char), 16, f);// stack_instr
//WRITE indices
fwrite( &indices[0], sizeof( u16 ), num_indices, f );
//WRITE vertices
fwrite( &vertices[0], sizeof( vertex_t ), num_vertices, f );
//transpose all matrices
mtranspose( &bind_shape_matrix[0] );
for( int i=0; i<num_bone; ++i ){
float res[16];
mtranspose( res,invm[i].data );
//PRE-MULTIPLY WITH BSM
mmul(invm[i].data, res, &bind_shape_matrix[0]);
for( int k=0;k<num_frame; ++k){
mtranspose(&v_keyframes[i][k*16] );
}
}
//fwrite( bind_shape_matrix.data(), sizeof(float),16,f);
//WRITE inverse matrices
fwrite( invm[0].data, sizeof(float),16*num_bone,f);
//KEYFRAMES
//create joints ( pos , dir/quat ) from matrices
//WRITE keyframes (and reorder frame wise, not bone wise)
//AND convert to joint quat
jq_t jq={};
std::vector<jq_t>jointquats;
jointquats.reserve(num_frame*num_bone);
for( int k=0;k<num_frame; ++k)
for( int i=0;i<num_bone; ++i){
quat_from_mat4(jq.quat,&v_keyframes[i][k*16]);
jq.pos[0]=v_keyframes[i][k*16+12];
jq.pos[1]=v_keyframes[i][k*16+13];
jq.pos[2]=v_keyframes[i][k*16+14];
jq.child[0]=jq.child[1]=jq.child[2]=jq.child[3]=0;
jointquats.push_back(jq);
//fwrite( &v_keyframes[i][k*16], sizeof(float),16,f);
}
fwrite(jointquats.data(), sizeof(jq_t),num_frame*num_bone,f);
//done
fclose(f);
//print stuff...
for( int i=0;i<16; ++i)
printf( "m = %.4f\tinv = %.4f\n",v_keyframes[0][i],
invm[0].data[i]);
printf("\nOUTPUT\n");
printf( "INDICES: %d\n", num_indices);
printf( "VERTICES: %d\n", num_vertices);
printf("BONES = %d\n", num_bone);
printf("FRAMES = %d\n", num_frame);
printf("STACK = \n\t");
for( int i=0;i<num_bone; ++i)
printf("%d, ",stack_instr[i]);
printf("\nHIERARCHY = \n\t");
for( int i=0;i<num_bone; ++i)
printf("%d, ",num_child[i]);
printf("\n");
}
return 0;
}
static int rbcholu(double *A,int N, int stride, double *UB, double *UT) {
int bs,bb,i,j,Nb,t,k,u,v,w,sc;
double *b,*x,*U12,*U12T;
double sum;
bs = (int) BLOCKSIZE;
bb = bs*bs;
if (N <= BLOCKSIZE) {
sc = rcholu(A,N,stride,UB);
if (sc == -1) {
return -1;
}
} else {
Nb = N - bs;
x = (double*) malloc(sizeof(double) * bs);
b = (double*) malloc(sizeof(double) * bs);
U12T = (double*) malloc(sizeof(double) * Nb * bs);
U12 = (double*) malloc(sizeof(double) * Nb * bs);
rcholu(A,bs,stride,UB); // U11
for (i =0; i < bs;++i) {
t = i *stride;
u = 0;
for(j = 0; j < N;++j) {
UT[u+i] = A[j+t];
u += bs;
}
}
for(k = 0; k < Nb;++k) {
u = k * bs;
for(i = 0; i < bs;++i) {
b[i] = UT[bb+u+i];
x[i] = 0.;
}
for (i = 0; i < bs;++i) {
t = i*bs;
sum = 0;
for (j = 0; j < i;++j) {
sum += UT[t+j] * x[j];
}
x[i] = (b[i] - sum) / UT[t+i];
}
v = bs + k;
for(i = 0; i < bs;++i) {
A[v] = x[i];
U12T[u+i] = x[i];
v += stride;
}
}
mtranspose(U12T,Nb,bs,U12);
mmult(U12T,U12,UT,Nb,bs,Nb);
free(U12T);
free(U12);
free(b);
free(x);
for (i = 0; i < Nb; ++i) {
u = bs * stride + bs + i * stride;
w = i * Nb;
for(j = i; j < Nb;j++) {
A[j + u] -= UT[j + w];
}
}
sc = rbcholu(A + bs * stride + bs,Nb,stride,UB,UT);
if (sc == -1) {
return -1;
}
}
return sc;
}