static t_vector calcul_normal2(t_env *rt, t_figure object, t_vector light_ray,
t_vector n)
{
t_vector tmp;
t_vector tmp2;
if (object.name == TRIANGLE || object.name == QUADRILATERAL
|| object.name == CUBE)
{
tmp = vecsub(&object.a, &object.b);
tmp2 = vecsub(&object.a, &object.c);
normalize(&tmp);
normalize(&tmp2);
n = vecprod(&tmp, &tmp2);
}
if (object.name == ELLIPSOIDE)
vecscale(&n, 0.5);
if (object.name == TORUS)
n = normal_torus(rt->inter, object);
if (object.name == PARABOL)
n = normale_parab(rt->inter);
rt->angle = vecdot(&n, &light_ray) / (sqrt(light_ray.x * light_ray.x
+ light_ray.y * light_ray.y + light_ray.z * light_ray.z) * sqrt(n.x
* n.x + n.y * n.y + n.z * n.z));
return (n);
}
int quadrilateral(t_vector r, t_figure tri, double *t, t_vector eye)
{
t_vector ab;
t_vector ac;
t_vector d;
double sol;
t_vector equ;
ab = vecsub(&tri.a, &tri.b);
ac = vecsub(&tri.a, &tri.c);
if ((sol = ab.z * r.y * ac.x - ab.y * r.z * ac.x - r.x * ab.z * ac.y
- r.y * ab.x * ac.z + r.z * ab.x * ac.y + r.x * ab.y * ac.z) == 0.00001)
return (0);
d = vecsub(&tri.a, &eye);
equ.x = -1 * (-1 * r.x * ab.y * d.z - r.z * ab.x * d.y + ab.y * r.z * d.x
+ r.x * ab.z * d.y + r.y * ab.x * d.z - ab.z * r.y * d.x) / sol;
equ.y = (-1 * r.y * d.x * ac.z + r.y * ac.x * d.z - ac.x * r.z * d.y
+ d.x * r.z * ac.y - r.x * ac.y * d.z + r.x * d.y * ac.z) / sol;
equ.z = -1 * (-1 * d.x * ab.z * ac.y + d.x * ab.y * ac.z + ab.x * ac.y * d.z
- ab.x * d.y * ac.z - ac.x * ab.y * d.z + ac.x * ab.z * d.y) / sol;
if (equ.y > 0.00001 && equ.x > 0.00001 && equ.y <= 1 && equ.x <= 1
&& equ.z < *t && equ.z > 0.00001)
{
*t = equ.z;
return (1);
}
return (0);
}
/** Computes the local area force (Dupin2007 eqn. 15) and adds this
force to the particle forces (see \ref tclcommand_inter).
@param p1,p2,p3 Pointers to triangle particles.
@param iaparams elastic area modulus ka, initial area A0 (see \ref tclcommand_inter).
@param force1 returns force of particle 1
@param force2 returns force of particle 2
@param force3 returns force of particle 3
@return 0
*/
inline int calc_area_force_local(Particle *p1, Particle *p2, Particle *p3,
Bonded_ia_parameters *iaparams, double force1[3], double force2[3], double force3[3]) //first-fold-then-the-same approach
{
int k;
double A, aa, h[3], rh[3], hn;
double p11[3],p22[3],p33[3];
int img[3];
memcpy(p11, p1->r.p, 3*sizeof(double));
memcpy(img, p1->l.i, 3*sizeof(int));
fold_position(p11, img);
memcpy(p22, p2->r.p, 3*sizeof(double));
memcpy(img, p2->l.i, 3*sizeof(int));
fold_position(p22, img);
memcpy(p33, p3->r.p, 3*sizeof(double));
memcpy(img, p3->l.i, 3*sizeof(int));
fold_position(p33, img);
for(k=0;k<3;k++) h[k]=1.0/3.0 *(p11[k]+p22[k]+p33[k]);
//volume+=A * -n[2]/dn * h[2];
A=area_triangle(p11,p22,p33);
aa=(A - iaparams->p.area_force_local.A0_l)/iaparams->p.area_force_local.A0_l;
//aminusb(3,h,p11,rh); // area_forces for each triangle node
vecsub(h,p11,rh); // area_forces for each triangle node
hn=normr(rh);
for(k=0;k<3;k++) {
force1[k] = iaparams->p.area_force_local.ka_l * aa * rh[k]/hn;
//(&part1)->f.f[k]+=force[k];
}
//aminusb(3,h,p22,rh); // area_forces for each triangle node
vecsub(h,p22,rh); // area_forces for each triangle node
hn=normr(rh);
for(k=0;k<3;k++) {
force2[k] = iaparams->p.area_force_local.ka_l * aa * rh[k]/hn;
//(&part2)->f.f[k]+=force[k];
}
//aminusb(3,h,p33,rh); // area_forces for each triangle node
vecsub(h,p33,rh); // area_forces for each triangle node
hn=normr(rh);
for(k=0;k<3;k++) {
force3[k] = iaparams->p.area_force_local.ka_l * aa * rh[k]/hn;
//(&part3)->f.f[k]+=force[k];
}
return 0;
}
void SRS::SolveAim(float psi_angle, Matrix R1)
{
float h1[3], N[3], angle;
Matrix S0, S1;
// Get the final hand position
evalcircle(c, u, v, radius, psi_angle, h1);
// Rotate ee_r1 to h1
crossproduct(N, ee_r1, h1);
unitize(N);
angle = angle_between_vectors(ee_r1, h1, N);
rotation_axis_to_matrix(N, angle, S0);
// Now rotate a0 to a
float a[3], a0[3];
vecsub(a, (float*)ee, h1);
unitize(a);
hmatmult(S1,Ry,S0);
vecmult0(a0, (float*)axis, S1);
cpvector(N, h1);
unitize(N);
angle = angle_between_vectors(a0, a, N);
rotation_axis_to_matrix(N, angle, S1);
hmatmult(R1, S0, S1);
}
//
// p = Projection of u onto plane whose normal is n
//
void project_plane(float p[3], float u[3], float n[3])
{
float un[3];
project(un, u, n);
vecsub(p, u, un);
}
float vecdist(const float t[], const float t2[])
{
float t3[3];
vecsub(t3, (float*)t, (float*)t2);
return _sqrt(DOT(t3,t3));
}
static t_vector calcul_normal(t_env *rt, t_figure object, t_vector light_ray)
{
t_vector n;
n = vecsub(&rt->tmp_center, &rt->tmp_inter);
n.y = (object.name == CYLINDER || object.name == L_CYLINDER
|| object.name == CONE || object.name == HYPERBOL
|| object.name == L_CONE) ? (0) : (n.y);
n = (object.name == PLANE) ? (rt->tmp_center) : (n);
if ((rt->disk_s == 2 && object.name == L_SPHERE) || (rt->disk_cy == 2
&& object.name == L_CYLINDER)
|| (rt->disk_co == 2 && object.name == L_CONE))
{
n.x = 0;
n.y = 1;
n.z = 0;
}
if ((rt->disk_cy == 3 && object.name == L_CYLINDER)
|| (rt->disk_co == 3 && object.name == L_CONE))
{
n.x = 0;
n.y = -1;
n.z = 0;
}
return (calcul_normal2(rt, object, light_ray, n));
}
static void calcul_reflec(t_env *rt, t_vector *n, t_vector *tmp_reflect,
t_vector *ray)
{
if (rt->object[rt->i2].name != PLANE)
*n = vecsub(&rt->object[rt->i2].center, &rt->inter);
else
*n = rt->object[rt->i2].center;
normalize(n);
if ((rt->disk_s == 2 && rt->object[rt->i2].name == L_SPHERE) ||
(rt->disk_cy == 2 && rt->object[rt->i2].name == L_CYLINDER))
{
n->x = 0;
n->y = 1;
n->z = 0;
}
if (rt->disk_cy == 3 && rt->object[rt->i2].name == L_CYLINDER)
{
n->x = 0;
n->y = -1;
n->z = 0;
}
tmp_reflect->x = ray->x;
tmp_reflect->y = ray->y;
tmp_reflect->z = ray->z;
ray->x = -2 * n->x * vecdot(n, tmp_reflect) + tmp_reflect->x;
ray->y = -2 * n->y * vecdot(n, tmp_reflect) + tmp_reflect->y;
ray->z = -2 * n->z * vecdot(n, tmp_reflect) + tmp_reflect->z;
}
float get_circle_equation(const float ee[3],
const float axis[3],
const float pos_axis[3],
float upper_len,
float lower_len,
float c[3],
float u[3],
float v[3],
float n[3])
{
float wn = norm((float *)ee);
float radius;
cpvector(n, ee);
unitize(n);
// Use law of cosines to get angle between first spherical joint
// and revolute joint
float alpha;
if (!law_of_cosines(wn, upper_len, lower_len, alpha))
return 0;
// center of circle (origin is location of first S joint)
vecscalarmult(c, n, _cos(alpha) * upper_len);
radius = _sin(alpha) * upper_len;
float temp[3];
//
// A little kludgy. If the goal is behind the joint instead
// of in front of it, we reverse the angle measurement by
// inverting the normal vector
//
if (DOT(n,pos_axis) < 0.0)
vecscalarmult(n,n,-1.0);
vecscalarmult(temp, n, DOT(axis,n));
vecsub(u, (float *)axis, temp);
unitize(u);
crossproduct(v, n, u);
#if 0
printf("Circle equation\n");
printf("c = [%lf,%lf,%lf]\n", c[0], c[1], c[2]);
printf("u = [%lf,%lf,%lf]\n", u[0], u[1], u[2]);
printf("v = [%lf,%lf,%lf]\n", v[0], v[1], v[2]);
printf("n = [%lf,%lf,%lf]\n", n[0], n[1], n[2]);
printf("r = %lf\n", radius);
#endif
return radius;
}
/** Velocity correction vectors are computed*/
void compute_vel_corr_vec(int *repeat_)
{
Bonded_ia_parameters *ia_params;
int i, j, k, c, np;
Cell *cell;
Particle *p, *p1, *p2;
double v_ij[3], r_ij[3], K, vel_corr;
for (c = 0; c < local_cells.n; c++)
{
cell = local_cells.cell[c];
p = cell->part;
np = cell->n;
for(i = 0; i < np; i++) {
p1 = &p[i];
k=0;
while(k<p1->bl.n)
{
ia_params = &bonded_ia_params[p1->bl.e[k++]];
if( ia_params->type == BONDED_IA_RIGID_BOND )
{
p2 = local_particles[p1->bl.e[k++]];
if (!p2) {
char *errtxt = runtime_error(128 + 2*ES_INTEGER_SPACE);
ERROR_SPRINTF(errtxt,"{054 rigid bond broken between particles %d and %d (particles not stored on the same node)} ",
p1->p.identity, p1->bl.e[k-1]);
return;
}
vecsub(p1->m.v, p2->m.v, v_ij);
get_mi_vector(r_ij, p1->r.p, p2->r.p);
if(fabs(scalar(v_ij, r_ij)) > ia_params->p.rigid_bond.v_tol)
{
K = scalar(v_ij, r_ij)/ia_params->p.rigid_bond.d2;
#ifdef MASS
K /= (PMASS(*p1) + PMASS(*p2));
#else
K /= 2.0;
#endif
for (j=0;j<3;j++)
{
vel_corr = K*r_ij[j];
p1->f.f[j] -= vel_corr*PMASS(*p2);
p2->f.f[j] += vel_corr*PMASS(*p1);
}
*repeat_ = *repeat_ + 1 ;
}
}
else
k += ia_params->num;
} //while loop
} //for i loop
} //for c loop
}
//
// R1 is the rotation matrix that takes the position of the
// R jt and the last S jt in the R1 frame to their locations
// in the global frame
//
void SRS::SolveR1(float angle, Matrix R1)
{
float p[3];
evaluate_circle(angle, p);
solve_R1(p_r1, ee_r1, p, ee, reciprocal_upper_len, R1);
#ifdef SRSDEBUG
float t1[3], t2[3];
vecsub(t1, p_r1, ee_r1);
vecsub(t2, p, ee);
printf("Elbow distance error is %lf\n",
DOT(p_r1, p_r1) - DOT(p,p));
printf("EE distance error is %lf\n",
DOT(ee,ee) - DOT(ee_r1,ee_r1));
printf("Distance between elbow and wrist error is %lf\n",
DOT(t1,t1) - DOT(t2,t2));
#endif
}
float SRS::PosToAngle(const float p[3])
{
// Find vector from center of circle to pos and project it onto circle
float cp[3], pp[3];
vecsub(cp, (float *) p, c);
project_plane(pp , cp, n);
// Find angle between u and pp. This is the swivel angle
return angle_between_vectors(u, pp, n);
}
/** Velocity correction vectors are computed*/
void compute_vel_corr_vec(int *repeat_)
{
Bonded_ia_parameters *ia_params;
int i, j, k, c, np;
Cell *cell;
Particle *p, *p1, *p2;
double v_ij[3], r_ij[3], K, vel_corr;
for (c = 0; c < local_cells.n; c++)
{
cell = local_cells.cell[c];
p = cell->part;
np = cell->n;
for(i = 0; i < np; i++) {
p1 = &p[i];
k=0;
while(k<p1->bl.n)
{
ia_params = &bonded_ia_params[p1->bl.e[k++]];
if( ia_params->type == BONDED_IA_RIGID_BOND )
{
p2 = local_particles[p1->bl.e[k++]];
if (!p2) {
runtimeErrorMsg() <<"rigid bond broken between particles " << p1->p.identity << " and " << p1->bl.e[k-1] << " (particles not stored on the same node)";
return;
}
vecsub(p1->m.v, p2->m.v, v_ij);
get_mi_vector(r_ij, p1->r.p, p2->r.p);
if(fabs(scalar(v_ij, r_ij)) > ia_params->p.rigid_bond.v_tol)
{
K = scalar(v_ij, r_ij)/ia_params->p.rigid_bond.d2;
#ifdef MASS
K /= ((*p1).p.mass + (*p2).p.mass);
#else
K /= 2.0;
#endif
for (j=0;j<3;j++)
{
vel_corr = K*r_ij[j];
p1->f.f[j] -= vel_corr*(*p2).p.mass;
p2->f.f[j] += vel_corr*(*p1).p.mass;
}
*repeat_ = *repeat_ + 1 ;
}
}
else
k += ia_params->num;
} //while loop
} //for i loop
} //for c loop
}
/* TODO: this function is not used anywhere. To be removed? */
double calc_mol_hydro_radius(Molecule mol)
{
int i, j, id1, id2;
double rh=0.0, diff_vec[3];
for(i=0; i<mol.part.n; i++) {
id1 = mol.part.e[i];
for(j=i+1; j<mol.part.n; j++) {
id2 = mol.part.e[i];
vecsub(partCfg[id1].r.p, partCfg[id2].r.p, diff_vec);
rh += 1.0/sqrt(sqrlen(diff_vec));
}
}
return 0.5*(mol.part.n*(mol.part.n-1))/rh;
}
/**Incorporates mass of each particle*/
double calc_mol_gyr_radius2(Molecule mol)
{
int i, id;
double rg=0.0, M=0.0, com[3], diff_vec[3];
calc_mol_center_of_mass(mol, com);
for(i=0; i<mol.part.n; i++) {
id = mol.part.e[i];
vecsub(partCfg[id].r.p, com, diff_vec);
rg += sqrlen(diff_vec);
M += PMASS(partCfg[id]);
}
return (rg/M);
}
//
// Form local coordinate system {x,y} from points p,q relative to the
// implicit origin 0. pscale is the reciprocal length of the p vector
// which as it turns out is already known. If the invert flag is true
// construct the transpose of the rotation matrix instead
//
inline void make_frame(const float p[3],
float p_scale,
const float q[3],
Matrix R,
int invert = 0)
{
float x[3], y[3], t[3];
// x vector is unit vector from origin to p
vecscalarmult(x, (float *)p, p_scale);
// y vector is unit perpendicular projection of q onto x
vecscalarmult(t, x, DOT(q,x));
vecsub(y, (float *) q, t);
unitize(y);
// z vector is x cross y
if (invert)
{
R[0][0] = x[0]; R[1][0] = x[1]; R[2][0] = x[2];
R[0][1] = y[0]; R[1][1] = y[1]; R[2][1] = y[2];
R[0][2] = x[1]*y[2] - x[2]*y[1];
R[1][2] = x[2]*y[0] - x[0]*y[2];
R[2][2] = x[0]*y[1] - x[1]*y[0];
}
else
{
R[0][0] = x[0]; R[0][1] = x[1]; R[0][2] = x[2];
R[1][0] = y[0]; R[1][1] = y[1]; R[1][2] = y[2];
R[2][0] = x[1]*y[2] - x[2]*y[1];
R[2][1] = x[2]*y[0] - x[0]*y[2];
R[2][2] = x[0]*y[1] - x[1]*y[0];
}
R[3][0] = R[3][1] = R[3][2] =
R[0][3] = R[1][3] = R[2][3] = 0;
R[3][3] = 1.0;
}
int calc_radial_density_map (int xbins,int ybins,int thetabins,double xrange,double yrange, double axis[3], double center[3], IntList *beadids, DoubleList *density_map, DoubleList *density_profile) {
int i,j,t;
int pi,bi;
int nbeadtypes;
int beadcount;
double vectprod[3];
double pvector[3];
double xdist,ydist,rdist,xav,yav,theta;
double xbinwidth,ybinwidth,binvolume;
double thetabinwidth;
double *thetaradii;
int *thetacounts;
int xindex,yindex,tindex;
xbinwidth = xrange/(double)(xbins);
ybinwidth = yrange/(double)(ybins);
nbeadtypes = beadids->n;
/* Update particles */
updatePartCfg(WITHOUT_BONDS);
/*Make sure particles are folded */
for (i = 0 ; i < n_part ; i++) {
fold_coordinate(partCfg[i].r.p,partCfg[i].l.i,0);
fold_coordinate(partCfg[i].r.p,partCfg[i].l.i,1);
fold_coordinate(partCfg[i].r.p,partCfg[i].l.i,2);
}
beadcount = 0;
xav = 0.0;
yav = 0.0;
for ( pi = 0 ; pi < n_part ; pi++ ) {
for ( bi = 0 ; bi < nbeadtypes ; bi++ ) {
if ( beadids->e[bi] == partCfg[pi].p.type ) {
/* Find the vector from the point to the center */
vecsub(center,partCfg[pi].r.p,pvector);
/* Work out x and y coordinates with respect to rotation axis */
/* Find the minimum distance of the point from the axis */
vector_product(axis,pvector,vectprod);
xdist = sqrt(sqrlen(vectprod)/sqrlen(axis));
/* Find the projection of the vector from the point to the center
onto the axis vector */
ydist = scalar(axis,pvector)/sqrt(sqrlen(axis));
/* Work out relevant indices for x and y */
xindex = (int)(floor(xdist/xbinwidth));
yindex = (int)(floor((ydist+yrange*0.5)/ybinwidth));
/*
printf("x %d y %d \n",xindex,yindex);
printf("p %f %f %f \n",partCfg[pi].r.p[0],partCfg[pi].r.p[1],partCfg[pi].r.p[2]);
printf("pvec %f %f %f \n",pvector[0],pvector[1],pvector[2]);
printf("axis %f %f %f \n",axis[0],axis[1],axis[2]);
printf("dists %f %f \n",xdist,ydist);
fflush(stdout);
*/
/* Check array bounds */
if ( (xindex < xbins && xindex > 0) && (yindex < ybins && yindex > 0) ) {
density_map[bi].e[ybins*xindex+yindex] += 1;
xav += xdist;
yav += ydist;
beadcount += 1;
} else {
// fprintf(stderr,"ERROR: outside array bounds in calc_radial_density_map"); fflush(NULL); errexit();
}
}
}
}
/* Now turn counts into densities for the density map */
for ( bi = 0 ; bi < nbeadtypes ; bi++ ) {
for ( i = 0 ; i < xbins ; i++ ) {
/* All bins are cylinders and therefore constant in yindex */
binvolume = PI*(2*i*xbinwidth + xbinwidth*xbinwidth)*yrange;
for ( j = 0 ; j < ybins ; j++ ) {
density_map[bi].e[ybins*i+j] /= binvolume;
}
}
}
/* if required calculate the theta density profile */
if ( thetabins > 0 ) {
/* Convert the center to an output of the density center */
xav = xav/(double)(beadcount);
yav = yav/(double)(beadcount);
thetabinwidth = 2*PI/(double)(thetabins);
thetaradii = (double*)malloc(thetabins*nbeadtypes*sizeof(double));
thetacounts = (int*)malloc(thetabins*nbeadtypes*sizeof(int));
for ( bi = 0 ; bi < nbeadtypes ; bi++ ) {
for ( t = 0 ; t < thetabins ; t++ ) {
thetaradii[bi*thetabins+t] = 0.0;
thetacounts[bi*thetabins+t] = 0.0;
}
}
/* Maybe there is a nicer way to do this but now I will just repeat the loop over all particles */
for ( pi = 0 ; pi < n_part ; pi++ ) {
for ( bi = 0 ; bi < nbeadtypes ; bi++ ) {
if ( beadids->e[bi] == partCfg[pi].p.type ) {
vecsub(center,partCfg[pi].r.p,pvector);
vector_product(axis,pvector,vectprod);
xdist = sqrt(sqrlen(vectprod)/sqrlen(axis));
ydist = scalar(axis,pvector)/sqrt(sqrlen(axis));
/* Center the coordinates */
xdist = xdist - xav;
ydist = ydist - yav;
rdist = sqrt(xdist*xdist+ydist*ydist);
if ( ydist >= 0 ) {
theta = acos(xdist/rdist);
} else {
theta = 2*PI-acos(xdist/rdist);
}
tindex = (int)(floor(theta/thetabinwidth));
thetaradii[bi*thetabins+tindex] += xdist + xav;
thetacounts[bi*thetabins+tindex] += 1;
if ( tindex >= thetabins ) {
fprintf(stderr,"ERROR: outside density_profile array bounds in calc_radial_density_map"); fflush(NULL); errexit();
} else {
density_profile[bi].e[tindex] += 1;
}
}
}
}
/* normalize the theta densities*/
for ( bi = 0 ; bi < nbeadtypes ; bi++ ) {
for ( t = 0 ; t < thetabins ; t++ ) {
rdist = thetaradii[bi*thetabins+t]/(double)(thetacounts[bi*thetabins+t]);
density_profile[bi].e[t] /= rdist*rdist;
}
}
free(thetaradii);
free(thetacounts);
}
// printf("done \n");
return ES_OK;
}
void collectdata(double *y, int* edgesrow, int *edgescol, double * edgesdist, int *relative, int d, int n, int ks, double *a, double *b, double *g)
{
double *diffy, *yij, *nid;
double gij;
int i, j, k;
int nn_start, nn_end, nn;
double *diffxjk, *diffyjk;
double gjk, *yjk, *tempA;
int relflg = *relative;
/* clear data area */
memset(a, 0, sizeof(double)*d*d*d*d);
memset(b, 0, sizeof(double)*d*d*n);
memset(g, 0, sizeof(double)*n);
/* temporary working space */
diffy = (double*)calloc(d, sizeof(double));
yij = (double*)calloc(d*d, sizeof(double));
tempA = (double*)calloc((d*d)*(d*d+1)/2, sizeof(double));
if(!diffy || !yij || !tempA) {
mexErrMsgTxt("Out of memory..cannot allocate working space..");
return;
}
if(relflg==0) {
for(i=0; i <n ; i++) {
/* figure out the nearest neighbors */
nn_start = edgescol[i]; nn_end = edgescol[i+1]-1;
if (nn_end > nn_start) {
for(nn=nn_start; nn<=nn_end; nn++) {
j = edgesrow[nn];
/* compute diffx and diffy */
/* vecsub(x+i*D, x+j*D, diffx, D);*/
gij = edgesdist[nn];
vecsub(y+i*d, y+j*d, diffy, d);
vecout(diffy, yij, d);
/* update A, b */
updateA(tempA, yij, 1.0,d*d);
updateB(b+i*d*d, gij, yij, d*d);
g[i] = g[i]+gij*gij;
}
}
}
} else {
/* reproducing the code above to some degree so that we don't have to
do if statement inside cache-intensive loop */
for(i=0; i <n ; i++) {
/* figure out the nearest neighbors */
nn_start = edgescol[i]; nn_end = edgescol[i+1]-1;
if (nn_end > nn_start) {
for(nn=nn_start; nn<=nn_end; nn++) {
j = edgesrow[nn];
gij=edgesdist[nn];
vecsub(y+i*d, y+j*d, diffy, d);
vecout(diffy, yij, d);
/* printf("diff: %f %f %f\n",yij[0],gij,edgesdist[nn]);*/
/* update A, b */
updateA(tempA, yij, 1.0/(gij*gij), d*d);
updateB(b+i*d*d, 1/gij, yij, d*d);
g[i] = g[i]+1;
}
}
}
}
/* make A symmetric */
recoverA(d*d, a, tempA);
}
static void vehicleSubTick(Chassis* c, float dt)
{
if (g_step==0) return;
if (g_step&1) g_step = 0;
vec3* chassisPos = &c->pose.v[3].v3;
vec3* x = &c->pose.v[0].v3;
vec3* y = &c->pose.v[1].v3;
vec3* z = &c->pose.v[2].v3;
// This bit is done by the physics engine
if(1)
{
vecaddscale(chassisPos, chassisPos, &c->vel, dt);
mtx rot;
matrixRotateByVelocity(&rot, &c->pose, &c->angVel, dt);
matrixCopy33(&c->pose, &rot);
}
// Damp
vecscale(&c->vel, &c->vel, expf(-dt*1.f));
vecscale(&c->angVel, &c->angVel, expf(-dt*1.f));
if (fabsf(c->angVel.x)<0.01f) c->angVel.x = 0.f;
if (fabsf(c->angVel.y)<0.01f) c->angVel.y = 0.f;
if (fabsf(c->angVel.z)<0.01f) c->angVel.z = 0.f;
ClampedImpulse frictionImpulse[numWheels][2];
c->steer = g_steer;
//g_steer *= expf(-dt*3.f);
//g_speed *= expf(-dt*3.f);
static float latf = 10.f;
static float angSpeed = 0.f;
if (g_handBrake>0.f)
{
g_handBrake *= expf(-4.f*dt);
g_speed *= expf(-4.f*dt);
if (g_handBrake < 0.1f)
{
g_handBrake = 0.f;
}
}
// Prepare
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
// Calculate the world position and offset of the suspension point
vec3mtx33mulvec3(&s->worldOffset, &c->pose, &s->offset);
vec3mtx43mulvec3(&s->worldDefaultPos, &c->pose, &s->offset);
w->pos = s->worldDefaultPos;
vec3 pointVel = getPointVel(c, &s->worldOffset);
vecadd(&w->vel, &w->vel, &pointVel);
float maxFriction0 = 2.0f * dt * c->mass * gravity * (1.f/(float)numWheels);
clampedImpulseInit(&frictionImpulse[i][0], maxFriction0);
float latfriction = 10.f;
float newAngSpeed = vecdot(z, &c->angVel);
float changeAngSpeed = (newAngSpeed - angSpeed)/dt;
angSpeed = newAngSpeed;
printf("changeAngSpeed = %f\n", changeAngSpeed);
float speed = fabsf(vecdot(y, &c->vel));
const float base = 0.5f;
if (g_speed>=0 && i>=2)
{
latfriction = 1.f*expf(-5.f*g_handBrake) + base;
// latfriction = 1.f*expf(-2.f*fabsf(speed*changeAngSpeed)) + base;
// if (angSpeed*g_steer < -0.1f)
// {
// latfriction = base;
// }
//if (g_steer == 0.f)
//{
// latf += (10.f - latf) * (1.f - exp(-0.1f*dt));
//}
//else
//{
// latf += (0.1f - latf) * (1.f - exp(-10.f*dt));
//}
//latfriction = latf;
}
else
{
latfriction = 10.f;
}
float maxFriction1 = latfriction * dt * c->mass * gravity * (1.f/(float)numWheels);
clampedImpulseInit(&frictionImpulse[i][1], maxFriction1);
vecset(&s->hitNorm, 0.f, 0.f, 1.f);
float steer = w->maxSteer*c->steer * (1.f + 0.3f*s->offset.x*c->steer);
vecscale(&w->wheelAxis, x, cosf(steer));
vecsubscale(&w->wheelAxis, &w->wheelAxis, y, sinf(steer));
w->frictionDir[0];
veccross(&w->frictionDir[0], z, &w->wheelAxis);
w->frictionDir[1] = w->wheelAxis;
w->angSpeed = -40.f*g_speed;
}
//=============
// VERBOSE
//=============
#define verbose false
#define dump if (verbose) printf
dump("==========================================\n");
dump("START ITERATION\n");
dump("==========================================\n");
float solverERP = numIterations>1 ? 0.1f : 1.f;
float changeSolverERP = numIterations>1 ? (1.f - solverERP)/(0.01f+ (float)(numIterations-1)) : 0.f;
for (int repeat=0; repeat<numIterations; repeat++)
{
dump(" == Start Iter == \n");
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
const bool axisError = true;
const bool friction = true;
// Friction
if (friction)
{
vec3 lateralVel;
vecaddscale(&lateralVel, &w->vel, &s->hitNorm, -vecdot(&s->hitNorm, &w->vel));
vecaddscale(&lateralVel, &lateralVel, &w->frictionDir[0], +w->angSpeed * w->radius);
{
int dir = 0;
float v = vecdot(&lateralVel, &w->frictionDir[dir]);
float denom = 1.f/w->mass + w->radius*w->radius*w->invInertia;
float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
vec3 impulseV;
vecscale(&impulseV, &w->frictionDir[dir], impulse);
vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
w->angSpeed = w->angSpeed + (impulse * w->radius * w->invInertia);
}
if (1)
{
int dir=1;
float v = vecdot(&lateralVel, &w->frictionDir[dir]);
float denom = 1.f/w->mass;
float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
vec3 impulseV;
vecscale(&impulseV, &w->frictionDir[dir], impulse);
vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
}
//dump("gound collision errorV = %f, vel of wheel after = %f\n", penetration, vecdot(&w->vel, &s->hitNorm));
}
if (axisError) // Axis Error
{
vec3 offset;
vecsub(&offset, &w->pos, chassisPos);
vec3 pointvel = getPointVel(c, &offset);
vec3 error;
vecsub(&error, &pointvel, &w->vel);
vecaddscale(&error, &error, z, -vecdot(&error, z));
vec3 norm;
if (vecsizesq(&error)>0.001f)
{
dump("axis error %f\n", vecsize(&error));
vecnormalise(&norm, &error);
float denom = computeDenominator(1.f/c->mass, 1.f/c->inertia, &offset, &norm) + 1.f/w->mass;
vecscale(&error, &error, -solverERP/denom);
addImpulseAtOffset(&c->vel, &c->angVel, 1.f/c->mass, 1.f/c->inertia, &offset, &error);
vecaddscale(&w->vel, &w->vel, &error, -solverERP/w->mass);
}
//dump("axis error vel of wheel after = %f, inline = %f\n", vecdot(&w->vel, &s->hitNorm), vecdot(&w->vel, &s->axis));
}
}
solverERP += changeSolverERP;
}
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
vec3 pointVel = getPointVel(c, s);
// Convert suspension wheel speed back to car space
vecsub(&w->vel, &w->vel, &pointVel);
}
}
void tsuroCardCreate(tsuroCard* card, int input[4][2])
{
bool okay = true;
memset(card, 0, sizeof(card));
memset(card->paths, 0xff, sizeof(card->paths));
tsuroCardSetAllColours(card, 1.f, 1.f, 1.f);
for (int i=0; i<4; i++)
{
int from = input[i][0];
int to = input[i][1];
if ((card->paths[from] & card->paths[to]) == -1)
{
card->paths[from] = to;
card->paths[to] = from;
}
else
{
printf("WARNING: Invalid input for card!\n");
okay = false;
}
}
if (okay)
{
// Generate the vector path
const float s = 0.5f*tsuroCardSize; // half width/scale of card
const float a = tsuroCardSize*(1.f/6.f); // position of connection points
static const float points[8][2] =
{
{-a, -s},
{+a, -s},
{+s, -a},
{+s, +a},
{+a, +s},
{-a, +s},
{-s, +a},
{-s, -a},
};
for (int i=0; i<8; i++) // We are doing this twice (but its easier this way!)
{
int from = i;
int to = card->paths[i];
vec3 f = {points[from][0], points[from][1], 0.f}; // from
vec3 t = {points[to][0], points[to][1], 0.f}; // to
if (to == tsuroEdgeOpposite1[from])
{
// Directly opposite
vec3 dv;
vecsub(&dv, &t, &f);
vecscale(&dv, &dv, 1.f/((float)(tsuroVectorPathSize-1)));
vec3* v = card->vpaths[i].centre;
v[0] = f;
for (int n=1; n<tsuroVectorPathSize; n++)
{
vecadd(&v[n], &v[n-1], &dv);
}
}
else if (to == tsuroEdgeOpposite2[from])
{
// Opposite wall
vec3 centre = {0.f, 0.f, 0.f};
vec3 adjacentFrom = {points[tsuroEdgeSame[from]][0], points[tsuroEdgeSame[from]][1], 0.f}; // The point that is on the same edge as from
vec3 adjacentTo = {points[tsuroEdgeSame[to]][0], points[tsuroEdgeSame[to]][1], 0.f}; // The point that is on the same edge as to
vec3 focal1, focal2;
vecmidpoint(&focal1, &adjacentFrom, &f);
vecmidpoint(&focal2, &adjacentTo, &t);
vec3 x,y;
vecsub(&x, ¢re, &focal1);
vecsub(&y, &f, &focal1);
generateQuarterCurve(&card->vpaths[i].centre[0], &focal1,&y,&x, tsuroVectorPathHalfSize+1, 1.5f);
vecsub(&x, ¢re, &focal2);
vecsub(&y, &t, &focal2);
generateQuarterCurve(&card->vpaths[i].centre[tsuroVectorPathHalfSize], &focal2,&x,&y, tsuroVectorPathHalfSize+1, 1.5f);
}
else if (to == tsuroEdgeSame[from])
{
vec3 centre = {0.f, 0.f, 0.f};
// Same wall
vec3 midpoint;
vecmidpoint(&midpoint, &f, &t);
vec3 x,y;
vecsub(&x, &f, &midpoint);
vecsub(&y, ¢re, &midpoint);
vecscale(&y, &y, 0.5f);
generateQuarterCurve(&card->vpaths[i].centre[0], &midpoint,&x,&y, tsuroVectorPathHalfSize+1, 0.8);
vecneg(&x, &x);
generateQuarterCurve(&card->vpaths[i].centre[tsuroVectorPathHalfSize], &midpoint,&y,&x, tsuroVectorPathHalfSize+1, 0.8);
}
else
{
vec3 focal;
switch(from)
{
case 0: focal.y = -s; break;
case 1: focal.y = -s; break;
case 2: focal.x = +s; break;
case 3: focal.x = +s; break;
case 4: focal.y = +s; break;
case 5: focal.y = +s; break;
case 6: focal.x = -s; break;
case 7: focal.x = -s; break;
}
switch(to)
{
case 0: focal.y = -s; break;
case 1: focal.y = -s; break;
case 2: focal.x = +s; break;
case 3: focal.x = +s; break;
case 4: focal.y = +s; break;
case 5: focal.y = +s; break;
case 6: focal.x = -s; break;
case 7: focal.x = -s; break;
}
vec3 x,y;
vecsub(&x, &f, &focal);
vecsub(&y, &t, &focal);
generateQuarterCurve(&card->vpaths[i].centre[0], &focal,&x,&y, tsuroVectorPathSize, 1.2f);
// printf("DUMP: from = %f %f %f\n", XYZ(f));
// vec3* v = card->vpaths[i].centre;
// for (int n=0; n<tsuroVectorPathSize; n++)
// {
// printf("%f %f %f\n", XYZp(v));
// v++;
// }
}
}
}
}
