void pl_project_to_plane(Polygon * pl,
const Plane * const pn,
const vec direction)
{
assert(pl);
assert(pn);
double d= v_dot(pn->normal, direction);
// otherwise projection is impossible
assert(fabs(d) > E);
vec tmp;
double * pt;
uint i;
double N;
for (i=0; i < pl->last; i++){
pt= pl->points[i].co;
v_sub(tmp, pt, pn->point);
N= -v_dot(pn->normal, tmp);
v_scale(tmp, direction, N/d);
v_add(pt, pt, tmp);
}
v_sub(tmp, pl->bb.min, pn->point);
N= -v_dot(pn->normal, tmp);
v_scale(tmp, direction, N/d);
v_add(pl->bb.min, pl->bb.min, tmp);
v_sub(tmp, pl->bb.max, pn->point);
N= -v_dot(pn->normal, tmp);
v_scale(tmp, direction, N/d);
v_add(pl->bb.max, pl->bb.max, tmp);
}
/* v_mltadd -- scalar/vector multiplication and addition
-- out = v1 + scale.v2 */
VEC *v_mltadd(VEC *v1,VEC *v2,double scale,VEC *out)
{
/* register u_int dim, i; */
/* Real *out_ve, *v1_ve, *v2_ve; */
if ( v1==(VEC *)NULL || v2==(VEC *)NULL )
error(E_NULL,"v_mltadd");
if ( v1->dim != v2->dim )
error(E_SIZES,"v_mltadd");
if ( scale == 0.0 )
return v_copy(v1,out);
if ( scale == 1.0 )
return v_add(v1,v2,out);
if ( v2 != out )
{
tracecatch(out = v_copy(v1,out),"v_mltadd");
/* dim = v1->dim; */
__mltadd__(out->ve,v2->ve,scale,(int)(v1->dim));
}
else
{
tracecatch(out = sv_mlt(scale,v2,out),"v_mltadd");
out = v_add(v1,out,out);
}
/************************************************************
out_ve = out->ve; v1_ve = v1->ve; v2_ve = v2->ve;
for ( i=0; i < dim ; i++ )
out->ve[i] = v1->ve[i] + scale*v2->ve[i];
(*out_ve++) = (*v1_ve++) + scale*(*v2_ve++);
************************************************************/
return (out);
}
//nearest points between two segments given by pi+veci, results given in ratio of vec [0 1]
int Reaching::FindSegmentsNearestPoints(cart_vec_t& p1,cart_vec_t& vec1,cart_vec_t& p2,cart_vec_t& vec2, float *nearest_point1, float *nearest_point2, float *dist){
CMatrix3_t mat;
CMatrix3_t invmat;
cart_vec_t& vec3,tmp,tmp1,tmp2,k,v2;
int i;
m_identity(mat);
v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not //
if(v_squ_length(vec3)<0.00001){
return 0;
}
v_scale(vec2,-1,v2);
m_set_v3_column(vec1,0,mat);
m_set_v3_column(v2,1,mat);
m_set_v3_column(vec3,2,mat);
m_inverse(mat,invmat);
v_sub(p2,p1,tmp);
v_transform_normal(tmp,invmat,k);
for(i=0;i<2;i++){
k[i] = max(min(1,k[i]),0);
}
v_scale(vec1,k[0],tmp);
v_add(p1,tmp,tmp1);
v_scale(vec2,k[1],tmp);
v_add(p2,tmp,tmp2);
v_sub(tmp2,tmp1,tmp);
*dist=v_length(tmp);
*nearest_point1 = k[0];
*nearest_point2 = k[1];
return 1;
}
void draw_triangle2(vect_t a,vect_t b,vect_t c,vect_t pos,rgb color){
int x0,y0,x1,y1,x2,y2;
a=v_add(a,pos);
b=v_add(b,pos);
c=v_add(c,pos);
project_coord(a,&x0,&y0);
project_coord(b,&x1,&y1);
project_coord(c,&x2,&y2);
draw_line_color(x0,y0,x1,y1,color);
draw_line_color(x1,y1,x2,y2,color);
draw_line_color(x2,y2,x0,y0,color);
}
void booz_sensors_model_accel_run( double time ) {
if (time < bsm.accel_next_update)
return;
static VEC* accel_ltp = VNULL;
accel_ltp = v_resize(accel_ltp, AXIS_NB);
/* substract gravity to acceleration in ltp frame */
accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp);
/* convert to body frame */
static VEC* accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
mv_mlt(bfm.dcm, accel_ltp, accel_body);
/* convert to imu frame */
static VEC* accel_imu = VNULL;
accel_imu = v_resize(accel_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, accel_body, accel_imu);
// printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
/* compute accel reading */
mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel);
v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
accel_error = v_zero(accel_error);
/* add a gaussian noise */
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
/* add per accel error reading */
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
/* round signal to account for adc discretisation */
RoundSensor(bsm.accel);
/* saturation */
BoundSensor(bsm.accel, 0, bsm.accel_resolution);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
bsm.accel_next_update += BSM_ACCEL_DT;
bsm.accel_available = TRUE;
}
int main(int argc, char ** argv) {
// Generate input vectors and destination vector
double *x = gen_array(ARRAY_SIZE);
double *y = gen_array(ARRAY_SIZE);
double *z = (double*) malloc(ARRAY_SIZE*sizeof(double));
unsigned int method = 0;
if (argc == 2)
method = atoi(argv[1]);
// Double check v_add is correct
if(!verify(method, x,y)) {
printf("v_add does not match oracle\n");
return 0;
}
// Test framework that sweeps the number of threads and times each ru
double start_time, run_time;
int num_threads = omp_get_max_threads();
for(int i=1; i<=num_threads; i++) {
omp_set_num_threads(i);
start_time = omp_get_wtime();
for(int j=0; j<REPEAT; j++) {
v_add(method, x,y,z);
}
run_time = omp_get_wtime() - start_time;
printf(" %d thread(s) took %f seconds\n",i,run_time);
}
}
static void kalman_correct(kalman_t *kf, float p, float v)
{
/* K = P * HT * inv(H * P * HT + R) */
m_mlt(kf->H, kf->P, kf->T0);
mmtr_mlt(kf->T0, kf->H, kf->T1);
m_add(kf->T1, kf->R, kf->T0);
m_inverse(kf->T0, kf->T1);
mmtr_mlt(kf->P, kf->H, kf->T0);
m_mlt(kf->T0, kf->T1, kf->K);
/* x = x + K * (z - H * x) */
mv_mlt(kf->H, kf->x, kf->t0);
v_set_val(kf->z, 0, p);
v_set_val(kf->z, 1, v);
v_sub(kf->z, kf->t0, kf->t1);
mv_mlt(kf->K, kf->t1, kf->t0);
v_add(kf->x, kf->t0, kf->t1);
v_copy(kf->t1, kf->x);
/* P = (I - K * H) * P */
m_mlt(kf->K, kf->H, kf->T0);
m_sub(kf->I, kf->T0, kf->T1);
m_mlt(kf->T1, kf->P, kf->T0);
m_copy(kf->T0, kf->P);
}
void idle(void) {
static int frames;
static float time;
float ifps;
vec3 v;
vec4 q0,q1,q2;
matrix m;
ifps = 1.0 / getfps();
if(!my_pause) angle += ifps * 360.0 / 4.0;
v_set(sin(angle * DEG2RAD / 3),cos(angle * DEG2RAD / 7),2,light);
v_set(0,0,1,v);
q_set(v,psi,q0);
v_set(0,1,0,v);
q_set(v,phi,q1);
q_multiply(q0,q1,q2);
q_to_matrix(q2,m);
v_set(dist,0,0,camera);
v_transform(camera,m,camera);
v_add(camera,mesh,camera);
frames++;
time += ifps;
if(time > 1.0) {
printf("%d frames %.2f fps\n",frames,(float)frames / time);
frames = 0;
time = 0;
}
}
/*********************************************************************
**
** Name: deqnt_msvq()
**
** Description:
**
** Dequantization using codebook indices with multi-stages
**
** Arguments:
**
** int16_t qout[] ---- (output) quantized data (Q15/Q17)
** int16_t codebook[] ---- codebooks, cb[0..numStages-1] (Q15/Q17)
** int16_t tos ---- the number of stages
** short *index ---- codebook index
**
** Return value: None
**
***********************************************************************/
void deqnt_msvq(int16_t qout[], const int16_t codebook[], int16_t tos,
const int16_t cb_size[], int16_t index[], int16_t dim)
{
register int16_t i;
const int16_t *cdbk_ptr;
int16_t temp;
int32_t L_temp;
/* ====== Clear output ====== */
v_zap(qout, dim);
/* ====== Add each stage ====== */
cdbk_ptr = codebook;
for (i = 0; i < tos; i++) {
/* v_add(qout, cdbk_ptr + index[i]*dim, dim); */
L_temp = melpe_L_mult(index[i], dim);
L_temp = melpe_L_shr(L_temp, 1);
temp = melpe_extract_l(L_temp);
v_add(qout, cdbk_ptr + temp, dim);
/* cdbk_ptr += cb_size[i] * dim; */
L_temp = melpe_L_mult(cb_size[i], dim);
L_temp = melpe_L_shr(L_temp, 1);
temp = melpe_extract_l(L_temp);
cdbk_ptr += temp;
}
}
/* v_linlist -- linear combinations taken from a list of arguments;
calling:
v_linlist(out,v1,a1,v2,a2,...,vn,an,NULL);
where vi are vectors (VEC *) and ai are numbers (double)
*/
VEC *v_linlist(VEC *out,VEC *v1,double a1,...)
{
va_list ap;
VEC *par;
double a_par;
if ( ! v1 )
return VNULL;
va_start(ap, a1);
out = sv_mlt(a1,v1,out);
while (par = va_arg(ap,VEC *)) { /* NULL ends the list*/
a_par = va_arg(ap,double);
if (a_par == 0.0) continue;
if ( out == par )
error(E_INSITU,"v_linlist");
if ( out->dim != par->dim )
error(E_SIZES,"v_linlist");
if (a_par == 1.0)
out = v_add(out,par,out);
else if (a_par == -1.0)
out = v_sub(out,par,out);
else
out = v_mltadd(out,par,a_par,out);
}
va_end(ap);
return out;
}
int main() {
// Generate input vectors and destination vector
double *x = gen_array(ARRAY_SIZE);
double *y = gen_array(ARRAY_SIZE);
double *z = (double*) malloc(ARRAY_SIZE*sizeof(double));
// Test framework that sweeps the number of threads and times each run
double start_time, run_time;
int num_threads = omp_get_max_threads();
for(int i=1; i<=num_threads; i++) {
omp_set_num_threads(i);
start_time = omp_get_wtime();
for(int j=0; j<REPEAT; j++)
v_add(x,y,z);
run_time = omp_get_wtime() - start_time;
// verify that output of v_add is correct
if(!verify(x,y)) {
printf("v_add does not match oracle\n");
return -1;
}
printf(" %d thread(s) took %f seconds\n",i,run_time);
}
return 0;
}
static void orthonormalize(float dcm[3][3])
{
/* Orthogonalize the i and j unit vectors (DCMDraft2 Eqn. 19). */
dcm_err = v_dotp(dcm[0], dcm[1]);
v_scale(dcm[1], -dcm_err/2, dcm_d[0]); /* i vector correction */
v_scale(dcm[0], -dcm_err/2, dcm_d[1]); /* i vector correction */
v_add(dcm[0], dcm_d[0], dcm[0]);
v_add(dcm[1], dcm_d[1], dcm[1]);
/* k = i x j */
v_crossp(dcm[0], dcm[1], dcm[2]);
/* Normalize all three vectors. */
v_norm(dcm[0]);
v_norm(dcm[1]);
v_norm(dcm[2]);
}
/* Compute the reflection of a vector on an object.
*
* the_object: The object to reflect off of.
* position: The position hit.
* direction: The direction of the vector to be reflected.
*
* Returns: the reflection of direction on the object at position.
*/
float* reflection_vector(object *the_object, float *position, float *direction) {
float *reflection = (float*)malloc(sizeof(float)*3);
float *normal = get_normal(the_object, position);
v_scale(normal, -2*v_dot(direction, normal), reflection);
v_add(reflection, direction, reflection);
v_unit(reflection, reflection);
return reflection;
}
void vhash(cons_t * cell)
{
char *vname;
int data;
cell = cell->cdr;
vname = cell->symbol;
data = sgmt_eval(cell->cdr).ivalue;
v_add(hash(vname), data);
}
void booz_sensors_model_mag_run( double time ) {
if (time < bsm.mag_next_update)
return;
/* rotate h to body frame */
static VEC *h_body = VNULL;
h_body = v_resize(h_body, AXIS_NB);
mv_mlt(bfm.dcm, bfm.h_ltp, h_body);
/* rotate to imu frame */
static VEC *h_imu = VNULL;
h_imu = v_resize(h_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, h_body, h_imu);
/* rotate to sensor frame */
static VEC *h_sensor = VNULL;
h_sensor = v_resize(h_sensor, AXIS_NB);
mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor);
mv_mlt(bsm.mag_sensitivity, h_sensor, bsm.mag);
v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
/* compute mag error readings */
static VEC *mag_error = VNULL;
mag_error = v_resize(mag_error, AXIS_NB);
/* add hard iron now ? */
mag_error = v_zero(mag_error);
/* add a gaussian noise */
mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];
mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];
mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
/* add error */
v_add(bsm.mag, mag_error, bsm.mag);
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
/* round signal to account for adc discretisation */
RoundSensor(bsm.mag);
bsm.mag_next_update += BSM_MAG_DT;
bsm.mag_available = TRUE;
}
// Double check if it is correct
int verify(double* x, double* y) {
double *z_v_add = (double*) malloc(ARRAY_SIZE*sizeof(double));
double *z_oracle = (double*) malloc(ARRAY_SIZE*sizeof(double));
v_add(x, y, z_v_add);
for(int i=0; i<ARRAY_SIZE; i++)
z_oracle[i] = x[i] + y[i];
for(int i=0; i<ARRAY_SIZE; i++)
if(z_oracle[i] != z_v_add[i])
return 0;
return 1;
}
void sm_frame(sm_t *sm,int from,int to,float frame) {
int i,frame0,frame1;
if(from == -1) from = 0;
if(to == -1) to = sm->num_frame;
frame0 = (int)frame;
frame -= frame0;
frame0 += from;
if(frame0 >= to) frame0 = ((frame0 - from) % (to - from)) + from;
frame1 = frame0 + 1;
if(frame1 >= to) frame1 = from;
for(i = 0; i < sm->num_bone; i++) {
float m0[16],m1[16];
float xyz0[3],xyz1[3],xyz[3],rot[4];
v_scale(sm->frame[frame0][i].xyz,1.0 - frame,xyz0);
v_scale(sm->frame[frame1][i].xyz,frame,xyz1);
v_add(xyz0,xyz1,xyz);
q_slerp(sm->frame[frame0][i].rot,sm->frame[frame1][i].rot,frame,rot);
m_translate(xyz,m0);
q_to_matrix(rot,m1);
m_multiply(m0,m1,sm->bone[i].matrix);
}
for(i = 0; i < sm->num_surface; i++) {
int j;
sm_surface_t *s = sm->surface[i];
for(j = 0; j < s->num_vertex; j++) {
int k;
v_set(0,0,0,s->vertex[j].xyz);
v_set(0,0,0,s->vertex[j].normal);
for(k = 0; k < s->vertex[j].num_weight; k++) {
float v[3];
sm_weight_t *w = &s->vertex[j].weight[k];
v_transform(w->xyz,sm->bone[w->bone].matrix,v);
v_scale(v,w->weight,v);
v_add(s->vertex[j].xyz,v,s->vertex[j].xyz);
v_transform_normal(w->normal,sm->bone[w->bone].matrix,v);
v_scale(v,w->weight,v);
v_add(s->vertex[j].normal,v,s->vertex[j].normal);
}
}
}
}
void draw_triangle(int *a,int *b,int *c,vect_t pos,rgb color){
int x0,y0,x1,y1,x2,y2;
vect_t x={a[0],a[1],a[2]+Z};
vect_t y={b[0],b[1],b[2]+Z};
vect_t z={c[0],c[1],c[2]+Z};
x=v_add(x,pos);
y=v_add(y,pos);
z=v_add(z,pos);
int q,w,e;
q=project_coord(x,&x0,&y0);
w=project_coord(y,&x1,&y1);
e=project_coord(z,&x2,&y2);
// if( !(q && w && e)) return;
draw_line_color(x0,y0,x1,y1,color);
draw_line_color(x1,y1,x2,y2,color);
draw_line_color(x2,y2,x0,y0,color);
}
static void kalman_predict(kalman_t *kf, float a)
{
/* x = A * x + B * u */
v_set_val(kf->u, 0, a);
mv_mlt(kf->A, kf->x, kf->t0);
mv_mlt(kf->B, kf->u, kf->t1);
v_add(kf->t0, kf->t1, kf->x);
/* P = A * P * AT + Q */
m_mlt(kf->A, kf->P, kf->T0);
mmtr_mlt(kf->T0, kf->A, kf->T1);
m_add(kf->T1, kf->Q, kf->P);
}
static int intersect(float fDst1, float fDst2,
const vec3_t a, const vec3_t b, vec3_t hit)
{
if ((fDst1 * fDst2) >= 0.0f)
return 0;
if (fDst1 == fDst2)
return 0;
v_sub(hit, b, a);
v_add(hit, a);
v_scale(hit, (-fDst1 / (fDst2 - fDst1)));
return 1;
}
// Double check if it is correct
int verify(unsigned int method, double* x, double* y) {
double *z_v_add = (double*) malloc(ARRAY_SIZE*sizeof(double));
double *z_oracle = (double*) malloc(ARRAY_SIZE*sizeof(double));
v_add(method, x, y, z_v_add);
for(int i=0; i<ARRAY_SIZE; i++) {
z_oracle[i] = x[i] + y[i];
}
for(int i=0; i<ARRAY_SIZE; i++) {
if(z_oracle[i] != z_v_add[i]) {
return 0;
}
}
return 1;
}
int main() {
int count = 0;
int errors = 0;
for (int i = 0; i < numbers_cnt; i++) {
uint32_t v = v_add(numbers[i], 0x4B00U<<16);
uint32_t fp = fp_add(numbers[i], 0x4B00U<<16, 0, 1);
if (v != fp && errors < 10) {
printf("flr: %08x => v %08x | fp %08x\n", numbers[i], v, fp);
}
errors += fp != v;
count += 1;
}
printf("flr: errors: %d tests: %d\n", errors, count);
return errors != 0;
}
void
pl_area_vec(const Polygon * pl, vec dest)
{
assert(pl);
v_zero(dest);
if (pl->last < 3) return;
uint i, j;
vec tmp;
for (i=pl->last - 1, j=0; j < pl->last; i=j++){
v_cross(tmp, pl->points[i].co, pl->points[j].co);
v_add(dest, dest, tmp);
}
}
//--------------------------------------------------------------------------
void Hqp_IpRedSpBKP::step(const Hqp_Program *qp, const VEC *z, const VEC *w,
const VEC *r1, const VEC *r2, const VEC *r3,
const VEC *r4, VEC *dx, VEC *dy, VEC *dz, VEC *dw)
{
VEC v;
assert((int)r1->dim == _n && (int)dx->dim == _n);
assert((int)r2->dim == _me && (int)dy->dim == _me);
assert((int)r3->dim == _m && (int)dz->dim == _m);
assert((int)r4->dim == _m && (int)dw->dim == _m);
// augment, copy, scale and permutate [r1;r2;r3] into _r12
// calculate, permutate and scale x
// augment r1
// temporary store (W^{-1}r_4 + ZW^{-1}r_3) in dz
v_part(_r12, 0, _n, &v);
v_slash(w, r4, dw);
v_star(_zw, r3, dz);
v_add(dw, dz, dz);
sp_mv_mlt(_CT, dz, &v);
v_sub(r1, &v, &v);
v_star(&v, _scale, &v);
v_copy(r2, v_part(_r12, _n, _me, &v));
px_vec(_J2QP, _r12, _r12);
spBKPsolve(_J, _pivot, _r12, _xy);
px_vec(_QP2J, _xy, _xy);
v_star(v_part(_xy, 0, _n, &v), _scale, dx);
v_copy(v_part(_xy, _n, _me, &v), dy);
sp_vm_mlt(_CT, dx, dw);
v_star(_zw, dw, dw);
v_sub(dz, dw, dz);
// calculate dw
sv_mlt(-1.0, r3, dw);
sp_mv_mltadd(dw, dx, qp->C, 1.0, dw);
// usage of _CT is numerically worse!
//sp_vm_mltadd(dw, dx, _CT, 1.0, dw);
}
/*
* Kalman filter update
*
* P is [4,4] and holds the covariance matrix
* R is [3,3] and holds the measurement weights
* C is [4,3] and holds the euler to quat tranform
* X is [4,1] and holds the estimated state as a quaterion
* Xe is [3,1] and holds the euler angles of the estimated state
* Xm is [3,1] and holds the measured euler angles
*/
static inline void
kalman_update(
m_t P,
m_t R,
m_t C,
v_t X,
const v_t Xe,
const v_t Xm
)
{
m_t T1;
m_t T2;
m_t E;
m_t K;
v_t err;
v_t temp;
/* E[3,3] = C[3,4] P[4,4] C'[4,3] + R[3,3] */
m_mult( T1, C, P, 3, 4, 4 );
m_transpose( T2, C, 3, 4 );
m_mult( E, T1, T2, 3, 4, 3 );
m_add( E, R, E, 3, 3 );
/* K[4,3] = P[4,4] C'[4,3] inv(E)[3,3] */
m_mult( T1, P, T2, 4, 4, 3 );
m33_inv( E, T2 );
m_mult( K, T1, T2, 4, 3, 3 );
/* X[4] = X[4] + K[4,3] ( Xm[3] - Xe[3] ) */
v_sub( err, Xm, Xe, 3 );
mv_mult( temp, K, err, 4, 3 );
v_add( X, temp, X, 4 );
/* P[4,4] = P[4,4] - K[4,3] C[3,4] P[4,4] */
m_mult( T1, K, C, 4, 3, 4 );
m_mult( T2, T1, P, 4, 4, 4 );
m_sub( P, T2, P, 4, 4 );
}
/*
* Compute the earliest time and position of the intersection of a
* sphere and a vertex.
*
* The sphere has radius R and moves along vector V from point P. The
* vertex moves along vector W from point Q in a coordinate system
* based at O.
*/
static float v_vert(float Q[3],
const float o[3],
const float q[3],
const float w[3],
const float p[3],
const float v[3], float r)
{
float O[3], P[3], V[3];
float t = LARGE;
v_add(O, o, q);
v_sub(P, p, O);
v_sub(V, v, w);
if (v_dot(P, V) < 0.0f)
{
t = v_sol(P, V, r);
if (t < LARGE)
v_mad(Q, O, w, t);
}
return t;
}
double rk4(double t, VEC *x, double h, VEC *Torq)
{
VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL;
VEC *temp=VNULL;
double step_size=0.5*h;
if ( x == VNULL )
error(E_NULL,"rk4");
v1 = v_get(x->dim);
v2 = v_get(x->dim);
v3 = v_get(x->dim);
v4 = v_get(x->dim);
temp = v_get(x->dim);
Sy_m(x, Torq, v1);
v_mltadd(x,v1,step_size,temp);
Sy_m(temp, Torq, v2);
v_mltadd(x,v2,step_size,temp);
Sy_m(temp, Torq, v3);
step_size = h;
v_mltadd(x, v3, step_size, temp);
Sy_m(temp, Torq, v4);
temp = v_copy(v1,temp);
v_mltadd(temp,v2,2.0,temp);
v_mltadd(temp,v3,2.0,temp);
v_add(temp,v4,temp);
v_mltadd(x,temp,(h/6.0),x);
return t+h;
V_FREE(v1);
V_FREE(v2);
V_FREE(v3);
V_FREE(v4);
V_FREE(temp);
}
VEC *iter_cgs(ITER *ip, VEC *r0)
#endif
{
STATIC VEC *p = VNULL, *q = VNULL, *r = VNULL, *u = VNULL;
STATIC VEC *v = VNULL, *z = VNULL;
VEC *tmp;
Real alpha, beta, nres, rho, old_rho, sigma, inner;
if (ip == INULL)
error(E_NULL,"iter_cgs");
if (!ip->Ax || !ip->b || !r0)
error(E_NULL,"iter_cgs");
if ( ip->x == ip->b )
error(E_INSITU,"iter_cgs");
if (!ip->stop_crit)
error(E_NULL,"iter_cgs");
if ( r0->dim != ip->b->dim )
error(E_SIZES,"iter_cgs");
if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;
p = v_resize(p,ip->b->dim);
q = v_resize(q,ip->b->dim);
r = v_resize(r,ip->b->dim);
u = v_resize(u,ip->b->dim);
v = v_resize(v,ip->b->dim);
MEM_STAT_REG(p,TYPE_VEC);
MEM_STAT_REG(q,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(v,TYPE_VEC);
if (ip->Bx) {
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
}
if (ip->x != VNULL) {
if (ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_cgs");
ip->Ax(ip->A_par,ip->x,v); /* v = A*x */
if (ip->Bx) {
v_sub(ip->b,v,v); /* v = b - A*x */
(ip->Bx)(ip->B_par,v,r); /* r = B*(b-A*x) */
}
else v_sub(ip->b,v,r); /* r = b-A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim); /* x == 0 */
ip->shared_x = FALSE;
if (ip->Bx) (ip->Bx)(ip->B_par,ip->b,r); /* r = B*b */
else v_copy(ip->b,r); /* r = b */
}
v_zero(p);
v_zero(q);
old_rho = 1.0;
for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {
inner = in_prod(r,r);
nres = sqrt(fabs(inner));
if (ip->steps == 0) ip->init_res = nres;
if (ip->info) ip->info(ip,nres,r,VNULL);
if ( ip->stop_crit(ip,nres,r,VNULL) ) break;
rho = in_prod(r0,r);
if ( old_rho == 0.0 )
error(E_BREAKDOWN,"iter_cgs");
beta = rho/old_rho;
v_mltadd(r,q,beta,u);
v_mltadd(q,p,beta,v);
v_mltadd(u,v,beta,p);
(ip->Ax)(ip->A_par,p,q);
if (ip->Bx) {
(ip->Bx)(ip->B_par,q,z);
tmp = z;
}
else tmp = q;
sigma = in_prod(r0,tmp);
if ( sigma == 0.0 )
error(E_BREAKDOWN,"iter_cgs");
alpha = rho/sigma;
v_mltadd(u,tmp,-alpha,q);
v_add(u,q,v);
(ip->Ax)(ip->A_par,v,u);
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z);
tmp = z;
}
else tmp = u;
v_mltadd(r,tmp,-alpha,r);
v_mltadd(ip->x,v,alpha,ip->x);
old_rho = rho;
}
#ifdef THREADSAFE
V_FREE(p);
V_FREE(q);
V_FREE(r);
V_FREE(u);
V_FREE(v);
V_FREE(z);
#endif
return ip->x;
}
MAT *iter_arnoldi_iref(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *s=VNULL, *tmp=VNULL;
VEC v; /* auxiliary vector */
int i,j;
Real h_val, c;
if (ip == INULL)
error(E_NULL,"iter_arnoldi_iref");
if ( ! ip->Ax || ! Q || ! ip->x )
error(E_NULL,"iter_arnoldi_iref");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_arnoldi_iref");
if ( Q->n != ip->x->dim || Q->m != ip->k )
error(E_SIZES,"iter_arnoldi_iref");
m_zero(Q);
H = m_resize(H,ip->k,ip->k);
m_zero(H);
u = v_resize(u,ip->x->dim);
r = v_resize(r,ip->k);
s = v_resize(s,ip->k);
tmp = v_resize(tmp,ip->x->dim);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(s,TYPE_VEC);
MEM_STAT_REG(tmp,TYPE_VEC);
v.dim = v.max_dim = ip->x->dim;
c = v_norm2(ip->x);
if ( c <= 0.0)
return H;
else {
v.ve = Q->me[0];
sv_mlt(1.0/c,ip->x,&v);
}
v_zero(r);
v_zero(s);
for ( i = 0; i < ip->k; i++ )
{
v.ve = Q->me[i];
u = (ip->Ax)(ip->A_par,&v,u);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* modified Gram-Schmidt */
r->ve[j] = in_prod(&v,u);
v_mltadd(u,&v,-r->ve[j],u);
}
h_val = v_norm2(u);
/* if u == 0 then we have an exact subspace */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
/* iterative refinement -- ensures near orthogonality */
do {
v_zero(tmp);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
s->ve[j] = in_prod(&v,u);
v_mltadd(tmp,&v,s->ve[j],tmp);
}
v_sub(u,tmp,u);
v_add(r,s,r);
} while ( v_norm2(s) > 0.1*(h_val = v_norm2(u)) );
/* now that u is nearly orthogonal to Q, update H */
set_col(H,i,r);
/* check once again if h_val is zero */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
if ( i == ip->k-1 )
{
*h_rem = h_val;
continue;
}
/* H->me[i+1][i] = h_val; */
m_set_val(H,i+1,i,h_val);
v.ve = Q->me[i+1];
sv_mlt(1.0/h_val,u,&v);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(s);
V_FREE(tmp);
#endif
return H;
}
static int game_step(const float g[3], float dt, int bt)
{
if (server_state)
{
float h[3];
/* Smooth jittery or discontinuous input. */
tilt.rx += (input_get_x() - tilt.rx) * dt / input_get_s();
tilt.rz += (input_get_z() - tilt.rz) * dt / input_get_s();
game_tilt_axes(&tilt, view.e);
game_cmd_tiltaxes();
game_cmd_tiltangles();
grow_step(&vary, dt);
game_tilt_grav(h, g, &tilt);
if (jump_b > 0)
{
jump_dt += dt;
/* Handle a jump. */
if (jump_dt >= 0.5f)
{
/* Translate view at the exact instant of the jump. */
if (jump_b == 1)
{
float dp[3];
v_sub(dp, jump_p, vary.uv->p);
v_add(view.p, view.p, dp);
jump_b = 2;
}
/* Translate ball and hold it at the destination. */
v_cpy(vary.uv->p, jump_p);
}
if (jump_dt >= 1.0f)
jump_b = 0;
}
else
{
/* Run the sim. */
float b = sol_step(&vary, h, dt, 0, NULL);
/* Mix the sound of a ball bounce. */
if (b > 0.5f)
{
float k = (b - 0.5f) * 2.0f;
if (got_orig)
{
if (vary.uv->r > grow_orig) audio_play(AUD_BUMPL, k);
else if (vary.uv->r < grow_orig) audio_play(AUD_BUMPS, k);
else audio_play(AUD_BUMPM, k);
}
else audio_play(AUD_BUMPM, k);
}
}
game_cmd_updball();
game_update_view(dt);
game_update_time(dt, bt);
return game_update_state(bt);
}
return GAME_NONE;
}
