/** This function returns the angle btw the triangle p1,p2,p3 and p2,p3,p4.
* Be careful, the angle depends on the orientation of the trianlges!
* You need to be sure that the orientation (direction of normal vector)
* of p1p2p3 is given by the cross product p2p1 x p2p3.
* The orientation of p2p3p4 must be given by p2p3 x p2p4.
*
* Example: p1 = (0,0,1), p2 = (0,0,0), p3=(1,0,0), p4=(0,1,0).
* The orientation of p1p2p3 should be in the direction (0,1,0)
* and indeed: p2p1 x p2p3 = (0,0,1)x(1,0,0) = (0,1,0)
* This function is called in the beginning of the simulation when creating
* bonds depending on the angle btw the triangles, the bending_force.
* Here, we determine the orientations by looping over the triangles
* and checking the correct orientation. So when defining the bonds by tcl command
* "part p2 bond xxxx p1 p3 p4", we correctly input the particle id's.
* So if you have the access to the order of particles, you are safe to call this
* function with exactly this order. Otherwise you need to check the orientations. */
inline double angle_btw_triangles(double *P1, double *P2, double *P3, double *P4) {
double phi;
double u[3],v[3],normal1[3],normal2[3]; //auxiliary variables
u[0] = P1[0] - P2[0]; // u = P2P1
u[1] = P1[1] - P2[1];
u[2] = P1[2] - P2[2];
v[0] = P3[0] - P2[0]; // v = P2P3
v[1] = P3[1] - P2[1];
v[2] = P3[2] - P2[2];
vector_product(u,v,normal1);
u[0] = P3[0] - P2[0]; // u = P2P3
u[1] = P3[1] - P2[1];
u[2] = P3[2] - P2[2];
v[0] = P4[0] - P2[0]; // v = P2P4
v[1] = P4[1] - P2[1];
v[2] = P4[2] - P2[2];
vector_product(u,v,normal2);
double tmp11,tmp22,tmp33;
// Now we compute the scalar product of n1 and n2 divided by the norms of n1 and n2
//tmp11 = dot(3,normal1,normal2); // tmp11 = n1.n2
tmp11 = scalar(normal1,normal2); // tmp11 = n1.n2
/*
tmp22 = normr(normal1);
tmp33 = normr(normal2);
tmp11 /= (tmp22*tmp33); // tmp11 = n1.n2/(|n1||n2|)
*/
tmp11 *= fabs(tmp11); // tmp11 = (n1.n2)^2
tmp22 = sqrlen(normal1); //tmp22 = |n1|^2
tmp33 = sqrlen(normal2); //tmp33 = |n1|^2
tmp11 /= (tmp22*tmp33); // tmp11 = (n1.n2/(|n1||n2|))^2
if (tmp11 > 0 ) {
tmp11 = sqrt(tmp11);
} else {
tmp11 = - sqrt(- tmp11);
}
if(tmp11>=1.) { tmp11=0.0;}
else if(tmp11<=-1.) { tmp11=M_PI;}
phi = M_PI - acos(tmp11); // The angle between the faces (not considering the orientation, always less or equal to Pi) is
// equal to Pi minus angle between the normals
// Now we need to determine, if the angle btw two triangles is less than Pi or more than Pi. To do this we check,
// if the point P4 lies in the halfspace given by trianlge P1P2P3 and the normal to this triangle. If yes, we have
// angle less than Pi, if not, we have angle more than Pi.
// General equation of the plane is n_x*x + n_y*y + n_z*z + d = 0 where (n_x,n_y,n_z) is the normal to the plane.
// Point P1 lies in the plane, therefore d = -(n_x*P1_x + n_y*P1_y + n_z*P1_z)
// Point P4 lies in the halfspace given by normal iff n_x*P4_x + n_y*P4_y + n_z*P4_z + d >= 0
tmp11 = - (normal1[0]*P1[0] + normal1[1]*P1[1] + normal1[2]*P1[2]);
if (normal1[0]*P4[0] + normal1[1]*P4[1] + normal1[2]*P4[2] + tmp11 < 0) phi = 2*M_PI - phi;
return phi;
}
Bool find_three_point_transform(float P1[3], float P2[3], float P3[3],
float Q1[3], float Q2[3], float Q3[3],
float mv[3], float R[3][3])
/* assumption: for each Pi!=Pj and Qi!=Qj */
{
float u[3][3], w[3][3];
int i,j,k;
vector_sub(P2,P1, u[0]);
vector_sub(P3,P1, u[1]);
vector_product(u[0], u[1], u[1]); /* u[1] = (P2-P1)x(P3-P1) */
vector_normalize(u[0]);
vector_normalize(u[1]);
vector_product(u[0], u[1], u[2]); /* u[2] |_ u[1] |_ u[2] */
if(vector_length(u[2])==0)
{
printf("three points transformation: the first three points are colinear !!!\n");
return False;
}
vector_sub(Q2,Q1, w[0]);
vector_sub(Q3,Q1, w[1]);
vector_product(w[0], w[1], w[1]); /* w[1] = (Q2-Q1)x(Q3-Q1) */
vector_normalize(w[0]);
vector_normalize(w[1]);
vector_product(w[0], w[1], w[2]); /* w[2] |_ w[1] |_ w[2] */
if(vector_length(w[2])==0)
{
printf("three points transformation: the last three points are colinear !!!\n");
return False;
}
for(i=0; i<3; i++)
for(j=0; j<3; j++)
{
R[i][j]=0;
for(k=0; k<3; k++)
R[i][j]+=u[k][j]*w[k][i];
}
matrix3_vector_mult(R, P1, mv);
vector_sub(Q1, mv, mv); /* mv = Q1 - R*P1 */
return True;
}
/* Hypothesis:
- either nmat is a matrix allocated with _matrix_alloc_int,
and his coefficients are not initialized,
- or nmat==mat
*/
static
matrix_t* matrix_assign_variable(pk_internal_t* pk,
bool destructive,
matrix_t* mat,
ap_dim_t dim, numint_t* tab)
{
size_t i,j,var;
bool den;
matrix_t* nmat;
var = pk->dec + dim;
den = numint_cmp_int(tab[0],1)>0;
nmat =
destructive ?
mat :
_matrix_alloc_int(mat->nbrows,mat->nbcolumns,false);
nmat->_sorted = false;
for (i=0; i<mat->nbrows; i++){
/* product for var column */
vector_product(pk,pk->matrix_prod,
mat->p[i],
tab,mat->nbcolumns);
/* columns != var */
if (!destructive){
/* Functional */
numint_init_set(nmat->p[i][0],mat->p[i][0]);
for (j=1; j<mat->nbcolumns; j++){
if (j!=var){
numint_init_set(nmat->p[i][j],mat->p[i][j]);
if (den){
numint_mul(nmat->p[i][j],mat->p[i][j],tab[0]);
}
}
}
}
else {
/* Side-effect */
for (j=0; j<mat->nbcolumns; j++){
if (j!=var){
if (den)
numint_mul(nmat->p[i][j],mat->p[i][j],tab[0]);
else
numint_set(nmat->p[i][j],mat->p[i][j]);
}
}
}
/* var column */
if (!destructive)
numint_init_set(nmat->p[i][var],pk->matrix_prod);
else
numint_set(nmat->p[i][var],pk->matrix_prod);
matrix_normalize_row(pk,nmat,i);
}
return nmat;
}
/** Computes the area of triangle between vectors P1 and P2,
* by computing the crossproduct P1 x P2 and taking the half of its norm */
inline double area_triangle_new(double *P1, double *P2) {
double area;
double normal[3], n; //auxiliary variables
vector_product(P1,P2,normal);
n=normr(normal);
area = 0.5*n;
return area;
}
int main() {
vector v1 = { 2, 4, 6 };
vector v2 = { 3, 5, 9 };
vector v;
v = vector_product(v1, v2);
printf("v1 * v2 = %.2f\n", scalar_product(v1, v2));
printf("v1 x v2 = [%.2f, %.2f, %.2f]\n", v.x, v.y, v.z);
return 0;
}
bool do_segments_intersect(const point *A1, const point *A2,
const point *B1, const point *B2){
long double v_x = A2->x - A1->x,
v_y = A2->y - A1->y,
b_x = B1->x - A1->x,
b_y = B1->y - A1->y;
if(vectors_on_two_sides(vector_product(v_x, v_y, B2->x - A1->x, B2->y - A1->y), vector_product(v_x, v_y, b_x, b_y))){
b_x = -b_x;
b_y = -b_y;
v_x = B2->x - B1->x;
v_y = B2->y - B1->y;
if(vectors_on_two_sides(vector_product(v_x, v_y, b_x, b_y), vector_product(v_x, v_y, A2->x - B1->x, A2->y - B1->y))){
return true;
}else{
return false;
}
}else{
return false;
}
}
/** Computes the area of triangle between vectors P1,P2,P3,
* by computing the crossproduct P1P2 x P1P3 and taking the half of its norm */
inline double area_triangle(double *P1, double *P2, double *P3) {
double area;
double u[3],v[3],normal[3], n; //auxiliary variables
u[0] = P2[0] - P1[0]; // u = P1P2
u[1] = P2[1] - P1[1];
u[2] = P2[2] - P1[2];
v[0] = P3[0] - P1[0]; // v = P1P3
v[1] = P3[1] - P1[1];
v[2] = P3[2] - P1[2];
vector_product(u,v,normal);
n=normr(normal);
area = 0.5*n;
return area;
}
t_bool intersect_triangle(t_ray ray, t_triangle *obj, float *out)
{
t_tri tri;
tri.e1 = vector_substract(obj->pos2, obj->pos);
tri.e2 = vector_substract(obj->pos3, obj->pos);
tri.p = vector_product(ray.dir, tri.e2);
tri.det = vector_dotproduct(tri.e1, tri.p);
if (tri.det > -1e-4f && tri.det < 1e-4f)
return (FALSE);
tri.det = 1.f / tri.det;
tri.t = vector_substract(ray.pos, obj->pos);
tri.u = vector_dotproduct(tri.t, tri.p) * tri.det;
if (tri.u < 0. || tri.u > 1.)
return (FALSE);
tri.q = vector_product(tri.t, tri.e1);
tri.v = vector_dotproduct(ray.dir, tri.q) * tri.det;
if (tri.v < 0. || (tri.u + tri.v) > 1.)
return (FALSE);
*out = vector_dotproduct(tri.e2, tri.q) * tri.det;
if (*out > 1e-4f)
return (TRUE);
return (FALSE);
}
/** Calculates the dihedral angle between particle quadruple p1, p2,
p3 and p4. The dihedral angle is the angle between the planes
specified by the particle triples (p1,p2,p3) and (p2,p3,p4).
Vectors a, b and c are the bond vectors between consequtive particles.
If the a,b or b,c are parallel the dihedral angle is not defined in which
case the routine returns phi=-1. Calling functions should check for that
(Written by: Arijit Maitra) */
inline void calc_dihedral_angle(Particle *p1, Particle *p2, Particle *p3, Particle *p4,
double a[3], double b[3], double c[3],
double aXb[3], double *l_aXb, double bXc[3], double *l_bXc,
double *cosphi, double *phi)
{
int i;
get_mi_vector(a, p2->r.p, p1->r.p);
get_mi_vector(b, p3->r.p, p2->r.p);
get_mi_vector(c, p4->r.p, p3->r.p);
/* calculate vector product a X b and b X c */
vector_product(a, b, aXb);
vector_product(b, c, bXc);
/* calculate the unit vectors */
*l_aXb = sqrt(sqrlen(aXb));
*l_bXc = sqrt(sqrlen(bXc));
/* catch case of undefined dihedral angle */
if ( *l_aXb <= TINY_LENGTH_VALUE || *l_bXc <= TINY_LENGTH_VALUE ) { *phi = -1.0; *cosphi = 0; return;}
for (i=0;i<3;i++) {
aXb[i] /= *l_aXb;
bXc[i] /= *l_bXc;
}
*cosphi = scalar(aXb, bXc);
if ( fabs(fabs(*cosphi)-1) < TINY_SIN_VALUE ) *cosphi = dround(*cosphi);
/* Calculate dihedral angle */
*phi = acos(*cosphi);
if( scalar(aXb, c) < 0.0 ) *phi = (2.0*PI) - *phi;
}
// Distribute forces that have accumulated on virtual particles to the
// associated real particles
void distribute_mol_force()
{
// Iterate over all the particles in the local cells
Particle *p;
int i, np, c;
Cell *cell;
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++) {
// We only care about virtual particles
if (ifParticleIsVirtual(&p[i])) {
update_mol_pos_particle(&p[i]);
// First obtain the real particle responsible for this virtual particle:
Particle *p_real = vs_relative_get_real_particle(&p[i]);
// Get distance vector pointing from real to virtual particle, respecting periodic boundary i
// conditions
double d[3];
get_mi_vector(d,p[i].r.p,p_real->r.p);
// The rules for transfering forces are:
// F_realParticle +=F_virtualParticle
// T_realParticle +=f_realParticle \times (r_virtualParticle-r_realParticle)
// Calculate torque to be added on real particle
double tmp[3];
vector_product(d,p[i].f.f,tmp);
// Add forces and torques
int j;
// printf("Particle %d gets torque from %f %f %f of particle %d\n",p_real->p.identity, p[i].f.f[0], p[i].f.f[1],p[i].f.f[2], p[i].p.identity);
for (j=0;j<3;j++) {
p_real->f.torque[j]+=tmp[j];
// printf("%f ",tmp[j]);
p_real->f.f[j]+=p[i].f.f[j];
// Clear forces on virtual particle
p[i].f.f[j]=0;
}
}
}
}
}
// Update the vel of the given virtual particle as defined by the real
// particles in the same molecule
void update_mol_vel_particle(Particle *p)
{
// NOT TESTED!
// First obtain the real particle responsible for this virtual particle:
Particle *p_real = vs_relative_get_real_particle(p);
// Check, if a real particle was found
if (!p_real)
{
char *errtxt = runtime_error(128 + 3*ES_INTEGER_SPACE);
ERROR_SPRINTF(errtxt, "virtual_sites_relative.cpp - update_mol_pos_particle(): No real particle associated with virtual site.\n");
return;
}
// Calculate the quaternion defining the orientation of the vecotr connectinhg
// the virtual site and the real particle
// This is obtained, by multiplying the quaternion representing the director
// of the real particle with the quaternion of the virtual particle, which
// specifies the relative orientation.
double q[4];
multiply_quaternions(p_real->r.quat,p->r.quat,q);
// Calculate the director resulting from the quaternions
double director[3];
convert_quat_to_quatu(q,director);
// normalize
double l =sqrt(sqrlen(director));
// Division comes in the loop below
// Get omega of real particle in space-fixed frame
double omega_space_frame[3];
convert_omega_body_to_space(p_real,omega_space_frame);
// Obtain velocity from v=v_real particle + omega_real_particle \times director
vector_product(omega_space_frame,director,p->m.v);
int i;
// Add prefactors and add velocity of real particle
for (i=0;i<3;i++)
{
// Scale the velocity by the distance of virtual particle from the real particle
// Also, espresso stores not velocity but velocity * time_step
p->m.v[i] *= time_step * p->p.vs_relative_distance/l;
// Add velocity of real particle
p->m.v[i] += p_real->m.v[i];
}
}
void
set_rotate_axis(double *axis)
{
void normalize_vec(double *);
void vector_product(double *, double *, double *);
int i;
/* Set up local axis frame */
/* rotate_axis_n is parallel to rotate_axis */
for(i = 0; i < 3; i++)
{
rotate_axis_n[i] = axis[i];
}
normalize_vec(rotate_axis_n);
/* rotate_axis_l is perpendicular to rotate_axis_n and lies in the
001 plane */
if(fabs(rotate_axis_n[2]) == 1.0)
{
rotate_axis_l[0] = 1.0; /* Arbitrary */
rotate_axis_l[1] = 0.0;
rotate_axis_l[2] = 0.0;
}
else
{
rotate_axis_l[0] = rotate_axis_n[1];
rotate_axis_l[1] = -rotate_axis_n[0];
rotate_axis_l[2] = 0.0;
}
normalize_vec(rotate_axis_l);
/* m is perpendicular to l and n */
vector_product(rotate_axis_m, rotate_axis_n, rotate_axis_l);
}
void angularmomentum(int type, double *com)
{
int i, j;
double tmp[3];
double pre_factor;
com[0]=com[1]=com[2]=0.;
updatePartCfg(WITHOUT_BONDS);
for (j=0; j<n_part; j++)
{
if (type == partCfg[j].p.type)
{
vector_product(partCfg[j].r.p,partCfg[j].m.v,tmp);
pre_factor=PMASS(partCfg[j]);
for (i=0; i<3; i++) {
com[i] += tmp[i]*pre_factor;
}
}
}
return;
}
// plot the horizon in green
static void draw_horizon(struct FTR *f)
{
struct viewer_state *e = f->userdata;
// four control points in projective coordinates
double a[3] = {e->c[0][0], e->c[0][1], 1};
double b[3] = {e->c[1][0], e->c[1][1], 1};
double c[3] = {e->c[2][0], e->c[2][1], 1};
double d[3] = {e->c[3][0], e->c[3][1], 1};
// lines though the control points
double lab[3]; vector_product(lab, a, b);
double lcd[3]; vector_product(lcd, c, d);
double lad[3]; vector_product(lad, a, d);
double lbc[3]; vector_product(lbc, b, c);
// intersections of opposite sides (vanishing points)
double p[3]; vector_product(p, lab, lcd);
double q[3]; vector_product(q, lad, lbc);
// horizon := line through two vanishing points
double horizon[3]; vector_product(horizon, p, q);
// affine coordinates of points
for (int k = 0; k < 2; k++) {
p[k] /= p[2];
q[k] /= q[2];
}
p[2] = q[2] = 1;
// plot the horizon
double v[2][2];
map_view_to_window(e, v[0], p);
map_view_to_window(e, v[1], q);
if (hypot(hypot(v[0][0], v[0][1]), hypot(v[1][0], v[1][1])) < 1e5)
plot_segment_green(f, v[0][0], v[0][1], v[1][0], v[1][1]);
}
real StressMajorizationSmoother_smooth(StressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol) {
SparseMatrix Lw = sm->Lw, Lwd = sm->Lwd, Lwdd = NULL;
int i, j, m, *id, *jd, *iw, *jw, idiag, flag = 0, iter = 0;
real *w, *dd, *d, *y = NULL, *x0 = NULL, *x00 = NULL, diag, diff = 1, *lambda = sm->lambda, maxit, res, alpha = 0., M = 0.;
SparseMatrix Lc = NULL;
Lwdd = SparseMatrix_copy(Lwd);
m = Lw->m;
x0 = N_GNEW(dim*m,real);
if (!x0) goto RETURN;
x0 = MEMCPY(x0, x, sizeof(real)*dim*m);
y = N_GNEW(dim*m,real);
if (!y) goto RETURN;
id = Lwd->ia; jd = Lwd->ja;
d = (real*) Lwd->a;
dd = (real*) Lwdd->a;
w = (real*) Lw->a;
iw = Lw->ia; jw = Lw->ja;
/* for the additional matrix L due to the position constraints */
if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
get_edge_label_matrix(sm->data, m, dim, x, &Lc, &x00);
if (Lc) Lw = SparseMatrix_add(Lw, Lc);
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
alpha = ((real*) (sm->data))[0];
M = ((real*) (sm->data))[1];
}
while (iter++ < maxit_sm && diff > tol){
for (i = 0; i < m; i++){
idiag = -1;
diag = 0.;
for (j = id[i]; j < id[i+1]; j++){
if (i == jd[j]) {
idiag = j;
continue;
}
dd[j] = d[j]/distance_cropped(x, dim, i, jd[j]);
diag += dd[j];
}
assert(idiag >= 0);
dd[idiag] = -diag;
}
/* solve (Lw+lambda*I) x = Lwdd y + lambda x0 */
SparseMatrix_multiply_dense(Lwdd, FALSE, x, FALSE, &y, FALSE, dim);
if (lambda){/* is there a penalty term? */
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += lambda[i]*x0[i*dim+j];
}
}
}
if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += x00[i*dim+j];
}
}
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){/* this part can be done more efficiently using octree approximation */
uniform_stress_augment_rhs(m, dim, x, y, alpha, M);
}
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress1 = %f\n",get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 0));
#endif
maxit = sqrt((double) m);
if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
res = uniform_stress_solve(Lw, alpha, dim, x, y, 0.01, maxit, &flag);
} else {
res = SparseMatrix_solve(Lw, dim, x, y, 0.01, maxit, SOLVE_METHOD_CG, &flag);
}
if (flag) goto RETURN;
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress2 = %f\n",get_stress(m, dim, iw, jw, w, d, y, sm->scaling, sm->data, 0));
#endif
diff = total_distance(m, dim, x, y)/sqrt(vector_product(m*dim, x, x));
#ifdef DEBUG_PRINT
if (Verbose){
fprintf(stderr, "Outer iter = %d, res = %g Stress Majorization diff = %g\n",iter, res, diff);
}
#endif
MEMCPY(x, y, sizeof(real)*m*dim);
}
#ifdef DEBUG
_statistics[1] += iter-1;
#endif
RETURN:
SparseMatrix_delete(Lwdd);
if (Lc) {
SparseMatrix_delete(Lc);
SparseMatrix_delete(Lw);
}
if (x0) FREE(x0);
if (y) FREE(y);
if (x00) FREE(x00);
return diff;
}
void CalcVolumeForce()
{
// Loop over all particles on local node
for (int c = 0; c < local_cells.n; c++)
{
const Cell *const cell = local_cells.cell[c];
for (int i = 0; i < cell->n; i++)
{
Particle &p1 = cell->part[i];
// Check if particle has a BONDED_IA_IBM_TRIEL and a BONDED_IA_IBM_VOLUME_CONSERVATION
// Basically this loops over all triangles, not all particles
// First round to check for volume conservation and virtual
// Loop over all bonds of this particle
// Actually j loops over the bond-list, i.e. the bond partners (see particle_data.hpp)
int softID = -1;
double volRef, kappaV;
int j = 0;
while ( j < p1.bl.n )
{
const int type_num = p1.bl.e[j];
const Bonded_ia_parameters &iaparams = bonded_ia_params[type_num];
const int type = iaparams.type;
if ( type == BONDED_IA_IBM_VOLUME_CONSERVATION )
{
if ( !p1.p.isVirtual) { printf("Error. Encountered non-virtual particle with VOLUME_CONSERVATION_IBM\n"); exit(1); }
softID = iaparams.p.ibmVolConsParameters.softID;
volRef = iaparams.p.ibmVolConsParameters.volRef;
kappaV = iaparams.p.ibmVolConsParameters.kappaV;
}
// Iterate, increase by the number of partners of this bond + 1 for bond type
j += iaparams.num+1;
}
// Second round for triel
if ( softID > -1 )
{
j = 0;
while ( j < p1.bl.n)
{
const int type_num = p1.bl.e[j];
const Bonded_ia_parameters &iaparams = bonded_ia_params[type_num];
const int type = iaparams.type;
if ( type == BONDED_IA_IBM_TRIEL )
{
// Our particle is the leading particle of a triel
// Get second and third particle of the triangle
Particle *p2 = local_particles[p1.bl.e[j+1]];
Particle *p3 = local_particles[p1.bl.e[j+2]];
// Unfold position of first node
// this is to get a continuous trajectory with no jumps when box boundaries are crossed
double x1[3] = { p1.r.p[0], p1.r.p[1], p1.r.p[2] };
int img[3] = { p1.l.i[0], p1.l.i[1], p1.l.i[2] };
unfold_position(x1,img);
// Unfolding seems to work only for the first particle of a triel
// so get the others from relative vectors considering PBC
double a12[3];
get_mi_vector(a12, p2->r.p, x1);
double a13[3];
get_mi_vector(a13, p3->r.p, x1);
// Now we have the true and good coordinates
// Compute force according to eq. C.46 Krüger thesis
// It is the same as deriving Achim's equation w.r.t x
/* const double fact = kappaV * 1/6. * (IBMVolumesCurrent[softID] - volRef) / IBMVolumesCurrent[softID];
double x2[3];
double x3[3];
for (int i=0; i < 3; i++)
{
x2[i] = x1[i] + a12[i];
x3[i] = x1[i] + a13[i];
}
double n[3];
vector_product(x3, x2, n);
for (int k=0; k < 3; k++) p1.f.f[k] += fact*n[k];
vector_product(x1, x3, n);
for (int k=0; k < 3; k++) p2->f.f[k] += fact*n[k];
vector_product(x2, x1, n);
for (int k=0; k < 3; k++) p3->f.f[k] += fact*n[k];*/
// This is Dupin 2008. I guess the result will be very similar as the code above
double n[3];
vector_product(a12, a13, n);
const double ln = sqrt( n[0]*n[0] + n[1]*n[1] + n[2]*n[2] );
const double A = 0.5 * ln;
const double fact = kappaV * (VolumesCurrent[softID] - volRef) / VolumesCurrent[softID];
double nHat[3];
nHat[0] = n[0] / ln;
nHat[1] = n[1] / ln;
nHat[2] = n[2] / ln;
double force[3];
force[0] = -fact * A * nHat[0];
force[1] = -fact * A * nHat[1];
force[2] = -fact * A * nHat[2];
// Add forces
for (int k=0; k < 3; k++)
{
p1.f.f[k] += force[k];
p2->f.f[k] += force[k];
p3->f.f[k] += force[k];
}
}
// Iterate, increase by the number of partners of this bond + 1 for bond type
j += iaparams.num+1;
}
}
}
}
}
static real conjugate_gradient(Operator A, Operator precon, int n, real *x, real *rhs, real tol, int maxit, int *flag){
real *z, *r, *p, *q, res = 10*tol, alpha;
real rho = 1.0e20, rho_old = 1, res0, beta;
real* (*Ax)(Operator o, real *in, real *out) = A->Operator_apply;
real* (*Minvx)(Operator o, real *in, real *out) = precon->Operator_apply;
int iter = 0;
z = N_GNEW(n,real);
r = N_GNEW(n,real);
p = N_GNEW(n,real);
q = N_GNEW(n,real);
r = Ax(A, x, r);
r = vector_subtract_to(n, rhs, r);
res0 = res = sqrt(vector_product(n, r, r))/n;
#ifdef DEBUG_PRINT
if (Verbose && 0){
fprintf(stderr, " cg iter = %d, residual = %g\n", iter, res);
}
#endif
while ((iter++) < maxit && res > tol*res0){
z = Minvx(precon, r, z);
rho = vector_product(n, r, z);
if (iter > 1){
beta = rho/rho_old;
p = vector_saxpy(n, z, p, beta);
} else {
MEMCPY(p, z, sizeof(real)*n);
}
q = Ax(A, p, q);
alpha = rho/vector_product(n, p, q);
x = vector_saxpy2(n, x, p, alpha);
r = vector_saxpy2(n, r, q, -alpha);
res = sqrt(vector_product(n, r, r))/n;
#ifdef DEBUG_PRINT
if (Verbose && 0){
fprintf(stderr, " cg iter = %d, residual = %g\n", iter, res);
}
#endif
rho_old = rho;
}
FREE(z); FREE(r); FREE(p); FREE(q);
#ifdef DEBUG
_statistics[0] += iter - 1;
#endif
#ifdef DEBUG_PRINT
if (Verbose && 0){
fprintf(stderr, " cg iter = %d, residual = %g\n", iter, res);
}
#endif
return res;
}
/* #include "matrix_market.h" */
void power_method(SparseMatrix A, int random_seed, int maxit, real tol, real **eigv){
/* find the largest eigenvector of a matrix A. Result in eigv. if eigv == NULL; memory will be allocated.
maxium of maxit iterations will be done, and tol is the convergence criterion
This converges only if the largest eigenvector/value is real and the next largest eigenvalue separate from teh largest one
*/
int n;
real *v, *u;
int iter = 0;
real res, unorm;
int i;
assert(A->m == A->n);
assert(A->type = MATRIX_TYPE_REAL || A->type == MATRIX_TYPE_INTEGER);
n = A->m;
if (!(*eigv)) *eigv = MALLOC(sizeof(real)*n);
u = MALLOC(sizeof(real)*n);
srand(random_seed);
for (i = 0; i < n; i++) (*eigv)[i] = drand();
res = vector_product(n, *eigv, *eigv);
if (res > 0) res = 1/res;
for (i = 0; i < n; i++) (*eigv)[i] = (*eigv)[i]*res;
v = *eigv;
do {
SparseMatrix_multiply_vector(A, v, &u, FALSE);
unorm = vector_product(n, u, u);/* ||u||^2 */
unorm = sqrt(unorm);
if (unorm > 0) unorm = 1/unorm;
res = 0.;
for (i = 0; i < n; i++) {
u[i] = u[i]*unorm;
res = res + (u[i] - v[i])*(u[i] - v[i]);
v[i] = u[i];
}
/*
printf("=== %d === %f\n",iter,res);
printf("{");
{int j;
for (j = 0; j < MIN(10,n); j++){
if (j == n-1){
printf("%f",v[j]);
} else {
printf("%f,",v[j]);
}
}
printf("\n");
}
*/
} while (res/n > tol*tol && iter++ < maxit);
}
EXPORT int intersection_of_two_shock_polars(
Locstate st0,
Locstate st1,
float *abs_v,
float *p,
float *pmin,
float *pmax,
bool Cplus_w0,
bool Cplus_w1,
RIEMANN_SOLVER_WAVE_TYPE *wtype0,
RIEMANN_SOLVER_WAVE_TYPE *wtype1)
{
float v0[MAXD]; /* Velocities in the steady frame */
float v1[MAXD];
float M0sq, M1sq; /* Mach #'s squared in steady frame */
float theta01; /* turn angle vector v0 to v1 */
float v0_x_v1;
float v0_d_v1;
float p0, p1;
float p_upper, p_lower;
const float meps = 10.0*MACH_EPS;/*TOLERANCE*/
float eps_theta;
float eps_pressure;
#if !defined(UNRESTRICTED_THERMODYNAMICS)
float min_pressure;
#endif /* !defined(UNRESTRICTED_THERMODYNAMICS) */
int dim;
DT_ANG_PARAMS parameters;
static char yesstatus[] =
"Left intersection_of_two_shock_polars() status = YES\n";
static char nostatus[] =
"Left intersection_of_two_shock_polars() status = NO\n";
debug_print("shock_polar","Entered intersection_of_two_shock_polars()\n");
dim = Params(st0)->dim;
p0 = pressure(st0);
p1 = pressure(st1);
M0sq = mach_number_squared(st0,abs_v,v0);
M1sq = mach_number_squared(st1,abs_v,v1);
(void) vector_product(v0,v1,&v0_x_v1,dim);
v0_d_v1 = scalar_product(v0,v1,dim);
theta01 = atan2(v0_x_v1,v0_d_v1);
#if defined(DEBUG_GIPOLAR)
if (debugging("shock_polar"))
{
(void) printf("abs_v = (%g, %g)\n",abs_v[0],abs_v[1]);
(void) printf("Cplus_w0 = %s, Cplus_w1 = %s\n",
(Cplus_w0) ? "YES" : "NO",
(Cplus_w1) ? "YES" : "NO");
verbose_print_state("st0",st0);
(void) printf("v0 = (%g, %g), q0 = %g, M0 = %g\n",v0[0],v0[1],
mag_vector(v0,dim),sqrt(M0sq));
verbose_print_state("st1",st1);
(void) printf("v1 = (%g, %g), q1 = %g, M1 = %g\n\n",v1[0],v1[1],
mag_vector(v1,dim),sqrt(M1sq));
print_angle("theta01 =",theta01,"\n");
}
#endif /* defined(DEBUG_GIPOLAR) */
*wtype0 = *wtype1 = UNSET_RIEMANN_SOLVER_WAVE_TYPE;
if (!spolar_intersect_bounds(theta01,M0sq,M1sq,st0,
st1,pmin,pmax,&p_upper,&p_lower,
Cplus_w0,Cplus_w1,wtype0,wtype1))
{
if (debugging("shock_polar"))
{
(void) printf("WARNING in intersection_of_two_shock_polars(), "
"spolar_intersect_bounds() failed.\n");
debug_print("shock_polar",nostatus);
}
return NO;
}
#if !defined(UNRESTRICTED_THERMODYNAMICS)
min_pressure = max(Min_pressure(st0),Min_pressure(st1));
if (p_upper < min_pressure)
{
*p = min_pressure;
*wtype0 = *wtype1 = RAREFACTION;
debug_print("shock_polar",yesstatus);
return YES;
}
#endif /* !defined(UNRESTRICTED_THERMODYNAMICS) */
if (p_upper <= p0) *wtype0 = RAREFACTION;
if (p_lower >= p0) *wtype0 = SHOCK;
if (p_upper <= p1) *wtype1 = RAREFACTION;
if (p_lower >= p1) *wtype1 = SHOCK;
eps_theta = fabs(theta01)*EPS;
eps_theta = max(meps,eps_theta);
eps_pressure = max(p0,p1)*EPS;
eps_pressure = max(meps,eps_pressure);
set_parameters(parameters,Cplus_w0,Cplus_w1,st0,st1,M0sq,M1sq);
if (find_root(diff_turn_ang,(POINTER)¶meters,theta01,p,p_lower,
p_upper,eps_theta,eps_pressure) == FUNCTION_FAILED)
{
if (debugging("shock_polar"))
{
(void) printf("WARNING in intersection_of_two_shock_polars()");
(void) printf(", No intersection of shock polars.\n");
}
debug_print("shock_polar",nostatus);
return NO;
}
*wtype0 = (*p >= p0) ? SHOCK : RAREFACTION;
*wtype1 = (*p >= p1) ? SHOCK : RAREFACTION;
debug_print("shock_polar",yesstatus);
return YES;
} /*end intersection_of_two_shock_polars*/
/* ---------------------------------------------------------------------- */
static
matrix_t* matrix_assign_variables(pk_internal_t* pk,
matrix_t* mat,
ap_dim_t* tdim,
numint_t** tvec,
size_t size)
{
size_t i,j,eindex;
matrix_t* nmat = _matrix_alloc_int(mat->nbrows, mat->nbcolumns,false);
numint_t den;
/* Computing common denominator */
numint_init_set(den,tvec[0][0]);
for (i=1; i<size; i++){
numint_mul(den,den,tvec[i][0]);
}
if (numint_cmp_int(den,1)!=0){
/* General case */
numint_t* vden = vector_alloc(size);
for (i=0; i<size; i++){
numint_divexact(vden[i],den,tvec[i][0]);
}
/* Column 0: copy */
for (i=0; i<mat->nbrows; i++){
numint_init_set(nmat->p[i][0],mat->p[i][0]);
}
/* Other columns */
eindex = 0;
for (j=1; j<mat->nbcolumns; j++){
if (eindex < size && pk->dec + tdim[eindex] == j){
/* We are on an assigned column */
for (i=0; i<mat->nbrows; i++){ /* For each row */
vector_product(pk,pk->matrix_prod,
mat->p[i],
tvec[eindex],mat->nbcolumns);
numint_mul(pk->matrix_prod,pk->matrix_prod,vden[eindex]);
/* Put the result */
numint_init_set(nmat->p[i][j],pk->matrix_prod);
}
eindex++;
}
else {
/* We are on a normal column */
for (i=0; i<mat->nbrows; i++){ /* For each row */
numint_init(nmat->p[i][j]);
numint_mul(nmat->p[i][j],mat->p[i][j],den);
}
}
}
vector_free(vden,size);
}
else {
/* Special case: all denominators are 1 */
/* Column 0: copy */
for (i=0; i<mat->nbrows; i++){
numint_init_set(nmat->p[i][0],mat->p[i][0]);
}
/* Other columns */
eindex = 0;
for (j=1; j<mat->nbcolumns; j++){
if (eindex < size && pk->dec + tdim[eindex] == j){
/* We are on a assigned column */
for (i=0; i<mat->nbrows; i++){ /* For each row */
vector_product(pk,pk->matrix_prod,
mat->p[i],
tvec[eindex],mat->nbcolumns);
numint_init_set(nmat->p[i][j],pk->matrix_prod);
}
eindex++;
}
else {
/* We are on a normal column */
for (i=0; i<mat->nbrows; i++){ /* For each row */
numint_init_set(nmat->p[i][j],mat->p[i][j]);
}
}
}
}
numint_clear(den);
for (i=0; i<mat->nbrows; i++){
matrix_normalize_row(pk,nmat,i);
}
return nmat;
}
int main(void)
{
// Engine initialization
TinyRender::EngineManager engine;
engine.set_viewport(0, 0, width, height);
engine.set_depth(width);
// TODO: (alxe) simplify api!!
TinyRender::Vec3f light_vec;
light_vec[0] = 0.0; light_vec[1] = 0.0; light_vec[2] = -1.0;
engine.set_light_direction(light_vec);
// Shader creation
std::unique_ptr<TinyRender::Shader> shader = std::make_unique<TinyRender::DummyShader>(engine);
std::vector<std::vector<int>> z_buffer(height, std::vector<int>(width, std::numeric_limits<int>::min()));
TinyRender::TGAImage tga_image(width, height, TinyRender::TGAImage::ImageFormat_RGB);
TinyRender::Model model("../../Models/AfricanHead.obj");
TinyRender::TGAImage text_map("../../Models/AfricanHeadDiffuse.tga");
std::ofstream out_tga_file("../../Temp/TGATEST.tga", std::ios::binary);
// Buffer of screen points
std::array<TinyRender::Vec3i, 3> screen_points;
for (size_t tr_id = 0; tr_id < model.triangles().size(); ++tr_id)
{
const TinyRender::Vec3f& p1 = model.vertices()[model.triangles()[tr_id].vertices[0]];
const TinyRender::Vec3f& uv1 = model.uv_vertices()[model.triangles()[tr_id].uv_vertices[0]];
const TinyRender::Vec3f& p2 = model.vertices()[model.triangles()[tr_id].vertices[1]];
const TinyRender::Vec3f& uv2 = model.uv_vertices()[model.triangles()[tr_id].uv_vertices[1]];
const TinyRender::Vec3f& p3 = model.vertices()[model.triangles()[tr_id].vertices[2]];
const TinyRender::Vec3f& uv3 = model.uv_vertices()[model.triangles()[tr_id].uv_vertices[2]];
const TinyRender::Vec3i& p1i = engine.transform(p1);
const TinyRender::Vec3i& p2i = engine.transform(p2);
const TinyRender::Vec3i& p3i = engine.transform(p3);
const float u1x = uv1[0] * text_map.width();
const float u1y = uv1[1] * text_map.height();
const float u2x = uv2[0] * text_map.width();
const float u2y = uv2[1] * text_map.height();
const float u3x = uv3[0] * text_map.width();
const float u3y = uv3[1] * text_map.height();
TinyRender::Vec3f norm_vec = vector_product((p3 - p1), (p2 - p1));
norm_vec.normalize();
const float intense = light_vec * norm_vec;
if (intense > 0)
{
TinyRender::render_triangle(p1i[0], p1i[1], p1i[2], u1x, u1y,
p2i[0], p2i[1], p2i[2], u2x, u2y,
p3i[0], p3i[1], p3i[2], u3x, u3y,
tga_image, text_map, z_buffer, intense);
}
}
tga_image << out_tga_file;
out_tga_file.close();
return 0;
}
void IBM_Tribend_CalcForce(Particle *p1, const int numPartners, Particle **const partners, const Bonded_ia_parameters &iaparams)
{
const tBendingMethod method = iaparams.p.ibm_tribend.method;
if ( method == NodeNeighbors ) CalcForceGompper(p1, numPartners, partners, iaparams.p.ibm_tribend.kb);
if ( method == TriangleNormals )
{
// move to separate function
if ( numPartners != 3 ) { printf("Error. TriangleNormals bending with != 3 partners!\n"); exit(1); }
Particle *p2 = partners[0];
Particle *p3 = partners[1];
Particle *p4 = partners[2];
// ************* This is Wolfgang's code **************
// with some corrections by Achim
//Get vectors making up the two triangles
double dx1[3], dx2[3], dx3[3];
get_mi_vector(dx1, p1->r.p, p3->r.p);
get_mi_vector(dx2, p2->r.p, p3->r.p);
get_mi_vector(dx3, p4->r.p, p3->r.p);
//Get normals on triangle; pointing outwards by definition of indices sequence
double n1[3], n2[3];
vector_product(dx1, dx2, n1);
vector_product(dx1, dx3, n2);
// Wolfgang here had a minus. It seems to work, so leave it in
n2[0]=-1*n2[0];
n2[1]=-1*n2[1];
n2[2]=-1*n2[2];
// Get 2*area of triangles out of the magnitude of the resulting normals and make the latter unity
const double Ai = sqrt(n1[0]*n1[0] + n1[1]*n1[1] + n1[2]*n1[2]);
n1[0] = n1[0]/Ai;
n1[1]=n1[1]/Ai;
n1[2]=n1[2]/Ai;
const double Aj = sqrt(n2[0]*n2[0] + n2[1]*n2[1] + n2[2]*n2[2]);
n2[0] = n2[0]/Aj;
n2[1]=n2[1]/Aj;
n2[2]=n2[2]/Aj;
//Get the prefactor for the force term
double sc = scalar(n1,n2);
if ( sc > 1.0) sc = 1.0;
//Get theta as angle between normals
double theta = acos(sc);
double direc[3];
vector_product(n1,n2,direc);
const double desc = scalar(dx1,direc);
if (desc < 0) theta = -1.0*theta;
const double DTh = theta - iaparams.p.ibm_tribend.theta0;
double Pre;
// Classical Wolfgang version
/* if ( theta > 0) Pre = 1.0*iaparams.p.ibm_tribend.kb * sin(DTh);
else Pre = -1.0*iaparams.p.ibm_tribend.kb * sin(DTh);*/
// Correct version with linearized sin
if ( theta > 0) Pre = 1.0*iaparams.p.ibm_tribend.kb * DTh;
else Pre = -1.0*iaparams.p.ibm_tribend.kb * DTh;
double v1l[3], v2l[3];
for (int i = 0; i < 3; i++)
{
v1l[i] = n2[i]-sc*n1[i];
v2l[i] = n1[i]-sc*n2[i];
}
double len = sqrt(sqrlen(v1l));
double v1[3], v2[3];
if (len>0) for ( int i = 0;i <3; i++) v1[i]=v1l[i]/len;
// Achim normalizes both with the length of v1, Wolfgang uses v1 and v2
// However, the length should be identical, so it does not matter
// if ( method == Krueger )
// len = sqrt(sqrlen(v2l));
if ( len > 0) for (int i = 0;i <3; i++) v2[i]=v2l[i]/len;
// Force for particle 1:
double tmp[3], tmp2[3], term1[3], term2[3];
get_mi_vector(tmp,p2->r.p,p3->r.p);
get_mi_vector(tmp2, p3->r.p, p4->r.p);
vector_product(tmp,v1, term1);
vector_product(tmp2,v2, term2);
for (int i = 0; i < 3; i++ )
p1->f.f[i] += Pre*(term1[i]/Ai + term2[i]/Aj);
// Force for particle 2:
get_mi_vector(tmp,p3->r.p,p1->r.p);
vector_product(tmp,v1, term1);
for (int i = 0; i < 3; i++)
p2->f.f[i] += Pre*(term1[i]/Ai);
// Force for Particle 3:
get_mi_vector(tmp,p1->r.p,p2->r.p);
get_mi_vector(tmp2, p4->r.p, p1->r.p);
vector_product(tmp,v1, term1);
vector_product(tmp2,v2, term2);
for (int i = 0; i < 3; i++)
p3->f.f[i] += Pre*(term1[i]/Ai + term2[i]/Aj);
// Force for Particle 4:
get_mi_vector(tmp,p1->r.p,p3->r.p);
vector_product(tmp,v2, term1);
for (int i = 0; i < 3; i++) p4->f.f[i] += Pre*(term1[i]/Aj);
}
}
void power_method(void (*matvec)(void *, int, int, real*, real **, int, int*),
void *A, int n, int K, int random_seed, int maxit, real tol, real **eigv, real **eigs){
/* find k-largest eigenvectors of a matrix A. Result in eigv. if eigv == NULL; memory will be allocated.
maxium of maxit iterations will be done, and tol is the convergence criterion
This converges only if the largest eigenvectors/values are real (e.g., if A is symmetric) and the
next largest eigenvalues separate from the largest ones
input:
matvec: a function point that takes a matrix M and a vector u, produce v = M.u
A: the matrix
n: dimension of matrix A
K: number of eigenes to find
random_seed: seed for eigenvector initialization
matrix: max number f iterations
tol: accuracy control
output:
eigv: eigenvectors. The i-th is at eigvs[i*n, i*(n+1) - 1]
eigs: eigenvalues. The i-th is at eigs[i]
Function PowerIteration (A – m × m matrix )
% This function computes u1, u2, . . . , uk, the first k eigenvectors of S.
const tol ← 0.001
for i = 1 to k do
. ui ← random
. ui ← ui/||ui||
. do
. vi ← ui
. % orthogonalize against previous eigenvectors
. for j = 1 to i − 1 do
. vi ← vi − (vi^Tvi)vj
. end for
. ui ← A vi/||A vi||
. while (ui^T vi < 1-tol) (halt when direction change is small)
. vi = ui
end for
return v1,v2,...
*/
real **v, *u, *vv;
int iter = 0;
real res, unorm;
int i, j, k;
real uij;
int flag;
K = MAX(0, MIN(n, K));
assert(K <= n && K > 0);
if (!(*eigv)) *eigv = MALLOC(sizeof(real)*n*K);
if (!(*eigs)) *eigs = MALLOC(sizeof(real)*K);
v = MALLOC(sizeof(real*)*K);
vv = MALLOC(sizeof(real)*n);
u = MALLOC(sizeof(real)*n);
srand(random_seed);
for (k = 0; k < K; k++){
//fprintf(stderr,"calculating eig k ==================== %d\n",k);
v[k] = &((*eigv)[k*n]);
for (i = 0; i < n; i++) u[i] = drand();
res = sqrt(vector_product(n, u, u));
if (res > 0) res = 1/res;
for (i = 0; i < n; i++) {
u[i] = u[i]*res;
v[k][i] = u[i];
}
/*
fprintf(stderr,"inital vec=");
for (i = 0; i < n; i++) fprintf(stderr,"%f,",u[i]);fprintf(stderr,"\n");
*/
iter = 0;
do {
/* normalize against previous eigens */
for (j = 0; j < k; j++){
uij = vector_product(n, u, v[j]);
for (i = 0; i < n; i++) {
u[i] = u[i] - uij *v[j][i];
}
}
matvec(A, n, n, u, &vv, FALSE, &flag);
assert(!flag);
/*
fprintf(stderr,"normalized aginst prev vec=");
for (i = 0; i < n; i++) fprintf(stderr,"%f,",u[i]);fprintf(stderr,"\n");
*/
unorm = vector_product(n, vv, vv);/* ||u||^2 */
unorm = sqrt(unorm);
(*eigs)[k] = unorm;
if (unorm > 0) {
unorm = 1/unorm;
} else {
// ||A.v||=0, so v must be an eigenvec correspond to eigenvalue zero
for (i = 0; i < n; i++) vv[i] = u[i];
unorm = sqrt(vector_product(n, vv, vv));
if (unorm > 0) unorm = 1/unorm;
}
res = 0.;
for (i = 0; i < n; i++) {
//res = MAX(res, ABS(vv[i]-(*eigs)[k]*u[i]));
u[i] = vv[i]*unorm;
res = res + u[i]*v[k][i];
v[k][i] = u[i];
}
//fprintf(stderr,"res=%g, tol = %g, res < 1-tol=%d\n",res, tol,res < 1 - tol);
} while (res < 1 - tol && iter++ < maxit);
//} while (iter++ < maxit);
//fprintf(stderr,"iter= %d, res=%f\n",iter, res);
}
FREE(u);
FREE(vv);
}
real StressMajorizationSmoother_smooth(StressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol) {
SparseMatrix Lw = sm->Lw, Lwd = sm->Lwd, Lwdd = NULL;
int i, j, k, m, *id, *jd, *iw, *jw, idiag, flag = 0, iter = 0;
real *w, *dd, *d, *y = NULL, *x0 = NULL, *x00 = NULL, diag, diff = 1, *lambda = sm->lambda, res, alpha = 0., M = 0.;
SparseMatrix Lc = NULL;
real dij, dist;
Lwdd = SparseMatrix_copy(Lwd);
m = Lw->m;
x0 = N_GNEW(dim*m,real);
if (!x0) goto RETURN;
x0 = MEMCPY(x0, x, sizeof(real)*dim*m);
y = N_GNEW(dim*m,real);
if (!y) goto RETURN;
id = Lwd->ia; jd = Lwd->ja;
d = (real*) Lwd->a;
dd = (real*) Lwdd->a;
w = (real*) Lw->a;
iw = Lw->ia; jw = Lw->ja;
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "initial stress = %f\n", get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 1));
#endif
/* for the additional matrix L due to the position constraints */
if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
get_edge_label_matrix(sm->data, m, dim, x, &Lc, &x00);
if (Lc) Lw = SparseMatrix_add(Lw, Lc);
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
alpha = ((real*) (sm->data))[0];
M = ((real*) (sm->data))[1];
}
while (iter++ < maxit_sm && diff > tol){
#ifdef GVIEWER
if (Gviewer) {
drawScene();
if (iter%2 == 0) gviewer_dump_current_frame();
}
#endif
if (sm->scheme != SM_SCHEME_STRESS_APPROX){
for (i = 0; i < m; i++){
idiag = -1;
diag = 0.;
for (j = id[i]; j < id[i+1]; j++){
if (i == jd[j]) {
idiag = j;
continue;
}
dist = distance(x, dim, i, jd[j]);
//if (d[j] >= -0.0001*dist){
// /* sometimes d[j] = 0 and ||x_i-x_j||=0*/
// dd[j] = d[j]/MAX(0.0001, dist);
if (d[j] == 0){
dd[j] = 0;
} else {
if (dist == 0){
dij = d[j]/w[j];/* the ideal distance */
/* perturb so points do not sit at the same place */
for (k = 0; k < dim; k++) x[jd[j]*dim+k] += 0.0001*(drand()+.0001)*dij;
dist = distance(x, dim, i, jd[j]);
}
dd[j] = d[j]/dist;
#if 0
/* if two points are at the same place, we do not want a huge entry,
as this cause problem with CG./ In any case,
at thw limit d[j] == ||x[i] - x[jd[j]]||,
or close. */
if (dist < -d[j]*0.0000001){
dd[j] = -10000.;
} else {
dd[j] = d[j]/dist;
}
#endif
}
diag += dd[j];
}
assert(idiag >= 0);
dd[idiag] = -diag;
}
/* solve (Lw+lambda*I) x = Lwdd y + lambda x0 */
SparseMatrix_multiply_dense(Lwdd, FALSE, x, FALSE, &y, FALSE, dim);
} else {
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] = 0;/* for stress_approx scheme, the whole rhs is calculated in stress_maxent_augment_rhs */
}
}
}
if (lambda){/* is there a penalty term? */
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += lambda[i]*x0[i*dim+j];
}
}
}
/* additional term added to the rhs */
switch (sm->scheme){
case SM_SCHEME_NORMAL_ELABEL: {
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += x00[i*dim+j];
}
}
break;
}
case SM_SCHEME_UNIFORM_STRESS:{/* this part can be done more efficiently using octree approximation */
uniform_stress_augment_rhs(m, dim, x, y, alpha, M);
break;
}
#if UNIMPEMENTED
case SM_SCHEME_MAXENT:{
#ifdef GVIEWER
if (Gviewer){
char *lab;
lab = MALLOC(sizeof(char)*100);
sprintf(lab,"maxent. alpha=%10.2g, iter=%d",stress_maxent_data_get_alpha(sm->data), iter);
gviewer_set_label(lab);
FREE(lab);
}
#endif
stress_maxent_augment_rhs_fast(sm, dim, x, y, &flag);
if (flag) goto RETURN;
break;
}
case SM_SCHEME_STRESS_APPROX:{
stress_approx_augment_rhs(sm, dim, x, y, &flag);
if (flag) goto RETURN;
break;
}
case SM_SCHEME_STRESS:{
#ifdef GVIEWER
if (Gviewer){
char *lab;
lab = MALLOC(sizeof(char)*100);
sprintf(lab,"pmds(k), iter=%d", iter);
gviewer_set_label(lab);
FREE(lab);
}
#endif
}
#endif /* UNIMPEMENTED */
default:
break;
}
#ifdef DEBUG_PRINT
if (Verbose) {
fprintf(stderr, "stress1 = %g\n",get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 1));
}
#endif
if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
res = uniform_stress_solve(Lw, alpha, dim, x, y, sm->tol_cg, sm->maxit_cg, &flag);
} else {
res = SparseMatrix_solve(Lw, dim, x, y, sm->tol_cg, sm->maxit_cg, SOLVE_METHOD_CG, &flag);
//res = SparseMatrix_solve(Lw, dim, x, y, sm->tol_cg, 1, SOLVE_METHOD_JACOBI, &flag);
}
if (flag) goto RETURN;
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress2 = %g\n",get_stress(m, dim, iw, jw, w, d, y, sm->scaling, sm->data, 1));
#endif
diff = total_distance(m, dim, x, y)/sqrt(vector_product(m*dim, x, x));
#ifdef DEBUG_PRINT
if (Verbose){
fprintf(stderr, "Outer iter = %d, cg res = %g, ||x_{k+1}-x_k||/||x_k|| = %g\n",iter, res, diff);
}
#endif
MEMCPY(x, y, sizeof(real)*m*dim);
}
#ifdef DEBUG
_statistics[1] += iter-1;
#endif
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "iter = %d, final stress = %f\n", iter, get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 1));
#endif
RETURN:
SparseMatrix_delete(Lwdd);
if (Lc) {
SparseMatrix_delete(Lc);
SparseMatrix_delete(Lw);
}
if (x0) FREE(x0);
if (y) FREE(y);
if (x00) FREE(x00);
return diff;
}
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;
}
/** calculate dihedral force between particles p1, p2 p3 and p4
Written by Arijit Maitra, adapted to new force interface by Hanjo,
more general new dihedral form by Ana.
*/
inline int calc_dihedral_force(Particle *p2, Particle *p1, Particle *p3, Particle *p4,
Bonded_ia_parameters *iaparams, double force2[3],
double force1[2], double force3[2])
{
int i;
/* vectors for dihedral angle calculation */
double v12[3], v23[3], v34[3], v12Xv23[3], v23Xv34[3], l_v12Xv23, l_v23Xv34;
double v23Xf1[3], v23Xf4[3], v34Xf4[3], v12Xf1[3];
/* dihedral angle, cosine of the dihedral angle */
double phi, cosphi, sinmphi_sinphi;
/* force factors */
double fac, f1[3], f4[3];
/* dihedral angle */
calc_dihedral_angle(p1, p2, p3, p4, v12, v23, v34, v12Xv23, &l_v12Xv23, v23Xv34, &l_v23Xv34, &cosphi, &phi);
/* dihedral angle not defined - force zero */
if ( phi == -1.0 ) {
for(i=0;i<3;i++) { force1[i] = 0.0; force2[i] = 0.0; force3[i] = 0.0; }
return 0;
}
/* calculate force components (directions) */
for(i=0;i<3;i++) {
f1[i] = (v23Xv34[i] - cosphi*v12Xv23[i])/l_v12Xv23;;
f4[i] = (v12Xv23[i] - cosphi*v23Xv34[i])/l_v23Xv34;
}
vector_product(v23, f1, v23Xf1);
vector_product(v23, f4, v23Xf4);
vector_product(v34, f4, v34Xf4);
vector_product(v12, f1, v12Xf1);
/* calculate force magnitude */
#ifdef OLD_DIHEDRAL
fac = iaparams->p.dihedral.bend * iaparams->p.dihedral.phase * iaparams->p.dihedral.mult;
#else
fac = -iaparams->p.dihedral.bend * iaparams->p.dihedral.mult;
#endif
if(fabs(sin(phi)) < TINY_SIN_VALUE) {
#ifdef OLD_DIHEDRAL
sinmphi_sinphi = iaparams->p.dihedral.mult * cos(2.0*PI - iaparams->p.dihedral.mult*phi)/cos(phi);
#else
/*(comes from taking the first term of the MacLaurin expansion of
sin(n*phi - phi0) and sin(phi) and then making the division).
The original code had a 2PI term in the cosine (cos(2PI - nPhi))
but I removed it because it wasn't doing anything. AnaVV*/
sinmphi_sinphi = iaparams->p.dihedral.mult*
cos(iaparams->p.dihedral.mult*phi - iaparams->p.dihedral.phase)/cosphi;
#endif
}
else {
#ifdef OLD_DIHEDRAL
sinmphi_sinphi = sin(iaparams->p.dihedral.mult*phi)/sin(phi);
#else
sinmphi_sinphi = sin(iaparams->p.dihedral.mult*phi - iaparams->p.dihedral.phase)/sin(phi);
#endif
}
fac *= sinmphi_sinphi;
/* store dihedral forces */
for(i=0;i<3;i++) {
force1[i] = fac*v23Xf1[i];
force2[i] = fac*(v34Xf4[i] - v12Xf1[i] - v23Xf1[i]);
force3[i] = fac*(v12Xf1[i] - v23Xf4[i] - v34Xf4[i]);
}
return 0;
}
int IBM_Tribend_SetParams(const int bond_type, const int ind1, const int ind2, const int ind3, const int ind4, const tBendingMethod method, const double kb, const bool flat)
{
// Create bond
make_bond_type_exist(bond_type);
// General parameters
bonded_ia_params[bond_type].type = BONDED_IA_IBM_TRIBEND;
// Specific parameters
bonded_ia_params[bond_type].p.ibm_tribend.method = method;
// Distinguish bending methods
if ( method == TriangleNormals )
{
double theta0;
if ( !flat )
{
// Compute theta0
Particle p1, p2, p3, p4;
get_particle_data(ind1, &p1);
get_particle_data(ind2, &p2);
get_particle_data(ind3, &p3);
get_particle_data(ind4, &p4);
//Get vectors of triangles
double dx1[3], dx2[3], dx3[3];
get_mi_vector(dx1, p1.r.p, p3.r.p);
get_mi_vector(dx2, p2.r.p, p3.r.p);
get_mi_vector(dx3, p4.r.p, p3.r.p);
//Get normals on triangle; pointing outwards by definition of indices sequence
double n1l[3], n2l[3];
vector_product(dx1, dx2, n1l);
vector_product(dx1, dx3, n2l);
// Wolfgang here had a minus. It seems to work, so leave it in
n2l[0] = -1*n2l[0];
n2l[1] = -1*n2l[1];
n2l[2] = -1*n2l[2];
double n1[3], n2[3];
unit_vector(n1l,n1);
unit_vector(n2l,n2);
//calculate theta by taking the acos of the scalar n1*n2
double sc = scalar(n1,n2);
if ( sc > 1.0) sc = 1.0;
theta0 = acos(sc);
double tmp[3];
vector_product(n1,n2,tmp);
const double desc = scalar(dx1,tmp);
if ( desc < 0) theta0 = 2.0*PI-theta0;
}
else theta0 = 0; // Flat
// Krüger always has three partners
bonded_ia_params[bond_type].num = 3;
bonded_ia_params[bond_type].p.ibm_tribend.theta0 = theta0;
// NOTE: This is the bare bending modulus used by the program.
// If triangle pairs appear only once, the total bending force should get a factor 2
// For the numerical model, a factor sqrt(3) should be added, see Gompper&Kroll J. Phys. 1996 and Krüger thesis
// This is an approximation, it holds strictly only for a sphere
bonded_ia_params[bond_type].p.ibm_tribend.kb = kb;
}
// Gompper
if ( method == NodeNeighbors )
{
// Interpret ind2 as number of partners
// Note: the actual partners are not set here, but must be set using the part command on the tcl level
if ( ind1 != 5 && ind1 != 6) { printf("Gompper bending with %d partners seems strange. Are you sure?\n", ind2); return ES_ERROR; }
bonded_ia_params[bond_type].num = ind1;
// Only flat eq possible, but actually this is ignored in the computation anyway
bonded_ia_params[bond_type].p.ibm_tribend.theta0 = 0;
bonded_ia_params[bond_type].p.ibm_tribend.kb = kb;
}
// Broadcast and return
mpi_bcast_ia_params(bond_type, -1);
return ES_OK;
}
/** Main method for calculation of bending forces described in literature
* @param p_ind1 particle object for particle 1
* @param p_ind2 particle object for particle 2
* @param p_ind3 particle object for particle 3
* @param p_ind3 particle object for particle 4
* @param iaparams parameters of the interaction such as max stretch, kb and theta0
* @param force forces in xyz on particle 1
* @param force2 forces in xyz on particle 2
* @param force3 forces in xyz on particle 3
* @param force4 forces in xyz on particle 4
*/
inline int calc_bending_force_ibm(Particle *p1, Particle *p2, Particle *p3, Particle *p4, Bonded_ia_parameters *iaparams, double force[3], double force2[3], double force3[3], double force4[3]) {
double theta, Ai, Aj;
double dx1[3], dx2[3], dx3[3], n1[3], n2[3];
double Pre, sc, len;
double v1l[3], v2l[3], v1[3], v2[3], tmp[3], tmp2[3], term1[3], term2[3];
double direc[3];
double desc, DTh;
int i;
//Get vectors making up the two triangles
get_mi_vector(dx1, p1->r.p, p3->r.p);
get_mi_vector(dx2, p2->r.p, p3->r.p);
get_mi_vector(dx3, p4->r.p, p3->r.p);
//printf("dx1: %lf %lf %lf\n", dx1[0], dx1[1], dx1[2]);
//printf("dx2: %lf %lf %lf\n", dx2[0], dx2[1], dx2[2]);
//printf("dx3: %lf %lf %lf\n", dx3[0], dx3[1], dx3[2]);
//Get normals on triangle; pointing outwards by definition of indices sequence
vector_product(dx1, dx2, n1);
vector_product(dx1, dx3, n2);
if(iaparams->p.bending_force_ibm.boo == 0) {
n2[0]=-1*n2[0]; n2[1]=-1*n2[1]; n2[2]=-1*n2[2];
}
//Get 2*area of triangles out of the magnitude of the resulting normals and make the latter unity
Ai = sqrt(n1[0]*n1[0] + n1[1]*n1[1] + n1[2]*n1[2]);
n1[0] = n1[0]/Ai; n1[1]=n1[1]/Ai; n1[2]=n1[2]/Ai;
Aj = sqrt(n2[0]*n2[0] + n2[1]*n2[1] + n2[2]*n2[2]);
n2[0] = n2[0]/Aj; n2[1]=n2[1]/Aj; n2[2]=n2[2]/Aj;
//printf("n1 = %lf %lf %lf\n", n1[0], n1[1], n1[2]);
//printf("n2 = %lf %lf %lf\n", n2[0], n2[1], n2[2]);
//printf("Ai = %lf Aj = %lf\n", Ai, Aj);
//Get the prefactor for the force term
sc = scalar(n1,n2);
if(sc>1.0) {
sc = 1.0;
}
//Get theta as angle between normals
theta = acos(sc);
//printf("Theta_Pure = %lf\n", theta);
//printf("n1: %lf %lf %lf\n", n1[0], n1[1], n1[2]);
//printf("n2: %lf %lf %lf\n", n2[0], n2[1], n2[2]);
vector_product(n1,n2,direc);
//printf("direc = %lf %lf %lf\n", direc[0], direc[1], direc[2]);
desc = scalar(dx1,direc);
//printf("dec = %lf\n", desc);
if(desc<0) {
theta = -1.0*theta;
}
//printf("theta = %lf theta0 = %lf\n", theta, iaparams->p.bending_force_ibm.theta0);
DTh = theta-iaparams->p.bending_force_ibm.theta0;
//printf("DTh = %lf\n", DTh);
if(isnan(theta)) {
printf("Triangle-Pair: %d %d %d %d\n", p1->p.identity, p2->p.identity, p3->p.identity, p4->p.identity);
printf("n1: %lf %lf %lf\n", n1[0], n1[1], n1[2]);
printf("n2: %lf %lf %lf\n", n2[0], n2[1], n2[2]);
printf("scalar: %lf\n", scalar(n1,n2));
printf("p1: %lf %lf %lf\n", p1->r.p[0], p1->r.p[1], p1->r.p[2]);
printf("p2: %lf %lf %lf\n", p2->r.p[0], p2->r.p[1], p2->r.p[2]);
printf("p3: %lf %lf %lf\n", p3->r.p[0], p3->r.p[1], p3->r.p[2]);
printf("p4: %lf %lf %lf\n", p4->r.p[0], p4->r.p[1], p4->r.p[2]);
}
//printf("%lf %lf\n", theta*TOANGLE, iaparams->p.bending_force_ibm.theta0*TOANGLE);
//printf("sc = %lf\n", sc);
if(theta>0) {
Pre = 1.0*iaparams->p.bending_force_ibm.kb * sin(DTh);
} else {
Pre = -1.0*iaparams->p.bending_force_ibm.kb * sin(DTh);
}
//printf("Pre = %lf kb=%lf\n", Pre, iaparams->p.bending_force_ibm.kb);
for(i=0; i<3; i++) {
v1l[i] = n2[i]-sc*n1[i];
v2l[i] = n1[i]-sc*n2[i];
}
//printf("v1l: %lf %lf %lf\n", v1l[0], v1l[1], v1l[2]);
//printf("v2l: %lf %lf %lf\n", v2l[0], v2l[1], v2l[2]);
len = sqrt(sqrlen(v1l));
//printf("len1 = %lf\n", len);
if(len>0) {
for(i=0;i <3; i++)
v1[i]=v1l[i]/len;
}
len = sqrt(sqrlen(v2l));
//printf("len2 = %lf\n", len);
if(len>0) {
for(i=0;i <3; i++)
v2[i]=v2l[i]/len;
}
//printf("v1: %lf %lf %lf\n", v1[0], v1[1], v1[2]);
//printf("v2: %lf %lf %lf\n", v2[0], v2[1], v2[2]);
//Force for particle 1:
get_mi_vector(tmp,p2->r.p,p3->r.p); get_mi_vector(tmp2, p3->r.p, p4->r.p);
vector_product(tmp,v1, term1); vector_product(tmp2,v2, term2);
//printf("tmp: %lf %lf %lf\n", tmp[0], tmp[1], tmp[2]);
//printf("tmp2: %lf %lf %lf\n", tmp2[0], tmp2[1], tmp2[2]);
//printf("p1f: ");
for(i=0;i<3;i++) {
force[i] = Pre*(term1[i]/Ai + term2[i]/Aj);
//printf("%lf ", Pre*(term1[i]/Ai + term2[i]/Aj));
}
//Force for particle 2:
get_mi_vector(tmp,p3->r.p,p1->r.p);
vector_product(tmp,v1, term1);
//printf("\np2f: ");
for(i=0;i<3;i++) {
force2[i] = Pre*(term1[i]/Ai);
//printf("%lf ", term1[i]/Ai);
}
//Force for Particle 3:
get_mi_vector(tmp,p1->r.p,p2->r.p); get_mi_vector(tmp2, p4->r.p, p1->r.p);
vector_product(tmp,v1, term1); vector_product(tmp2,v2, term2);
//printf("\np3f: ");
for(i=0;i<3;i++) {
force3[i] = Pre*(term1[i]/Ai + term2[i]/Aj);
//printf("%lf ", Pre*(term1[i]/Ai + term2[i]/Aj));
}
//Force for Particle 4:
get_mi_vector(tmp,p1->r.p,p3->r.p);
vector_product(tmp,v2, term1);
//printf("\np4f: ");
for(i=0;i<3;i++) {
force4[i] = Pre*(term1[i]/Aj);
//printf("%lf ", Pre*(term1[i]/Aj));
}
//printf("\n");
return 0;
}
void calc_comforce()
{
IA_parameters *ia_params;
std::vector<double> com0 (3), com1 (3);
double MofImatrix[9], diff[3];
double vect0[3], vect1[3], eva[3], eve[3], fvect[3];
Particle *p;
int np;
Cell *cell;
for (int t0=0; t0<n_particle_types-1; t0++) {
for (int t1=t0+1; t1<n_particle_types; t1++) {
ia_params = get_ia_param(t0,t1);
if (ia_params->COMFORCE_flag == 1) {
com0 = centerofmass(t0);
com1 = centerofmass(t1);
for (int i = 0; i < 3; i++) {
diff[i]=com1[i]-com0[i];
}
momentofinertiamatrix(t0, MofImatrix);
calc_eigenvalues_3x3(MofImatrix, eva);
/* perpendicular force */
if(ia_params->COMFORCE_dir == 1) {
calc_eigenvector_3x3(MofImatrix,eva[0],eve);
/*By doing two vector products find radial axis along the target system */
vector_product(eve,diff,vect0);
vector_product(vect0,eve,vect1);
/* normalize vect1, return is fvect */
unit_vector(vect1,fvect);
} else {
/* parallel force */
calc_eigenvector_3x3(MofImatrix,eva[0],fvect);
}
/* orient it along the com vector */
if (scalar(fvect,diff) < 0.) {
for (int i = 0; i < 3; i++) {
fvect[i] = -fvect[i];
}
}
/* Now apply the force */
for (int c = 0; c < local_cells.n; c++) {
cell = local_cells.cell[c];
p = cell->part;
np = cell->n;
for(int i = 0; i < np; i++) {
if(p[i].p.type==t0) {
for(int j = 0; j < 3; j++) {
p[i].f.f[j] -= ia_params->COMFORCE_fratio * ia_params->COMFORCE_force * fvect[j];
}
}
if(p[i].p.type==t1) {
for (int j = 0; j < 3; j++) {
p[i].f.f[j] += ia_params->COMFORCE_force * fvect[j];
}
}
}
}
/*end of force application */
}
}
}
}
/** Computes the four body overlapped dihedral interaction force.
Adds this force to the particle forces in forces.hpp (see \ref tclcommand_inter).
@param p1, p2, p3, p4 define the angle between the planes p1,p2,p3 and p2,p3,p4
@param iaparams bond type number of the angle interaction (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
Needs feature OVERLAPPED compiled in (see \ref config.hpp).
*/
inline int calc_overlap_dihedral_force(Particle *p2, Particle *p1,
Particle *p3, Particle *p4, Bonded_ia_parameters *iaparams,
double force2[3], double force1[3], double force3[3])
{
int i;
/* vectors for dihedral angle calculation */
double v12[3], v23[3], v34[3], v12Xv23[3], v23Xv34[3], l_v12Xv23, l_v23Xv34;
double v23Xf1[3], v23Xf4[3], v34Xf4[3], v12Xf1[3];
/* dihedral angle, cosine of the dihedral angle */
double phi, cosphi, sinmphi_sinphi;
/* force factors */
double fac, f1[3], f4[3];
int ig;
double f_dihe = 0.;
/* dihedral angle */
calc_dihedral_angle(p1, p2, p3, p4, v12, v23, v34, v12Xv23, &l_v12Xv23, v23Xv34, &l_v23Xv34, &cosphi, &phi);
/* dihedral angle not defined - force zero */
if ( phi == -1.0 ) {
for(i=0;i<3;i++) { force1[i] = 0.0; force2[i] = 0.0; force3[i] = 0.0; }
return 0;
}
/* calculate force components (directions) */
for(i=0;i<3;i++) {
f1[i] = (v23Xv34[i] - cosphi*v12Xv23[i])/l_v12Xv23;;
f4[i] = (v12Xv23[i] - cosphi*v23Xv34[i])/l_v23Xv34;
}
vector_product(v23, f1, v23Xf1);
vector_product(v23, f4, v23Xf4);
vector_product(v34, f4, v34Xf4);
vector_product(v12, f1, v12Xf1);
/* calculate force magnitude */
//fac = sum_(i=1,N) { a_i * c_i * c_i * cos(c_i*x + b_i)/cos(phi) }
//fac = sum_(i=1,N) { a_i * c_i * sin(c_i*phi + b_i) /sin(phi)}
for(ig=0; ig<iaparams->p.overlap.noverlaps; ig++) {
fac = iaparams->p.overlap.para_a[ig] * iaparams->p.overlap.para_c[ig];
if(fabs(sin(phi)) < TINY_SIN_VALUE) {
/*(comes from taking the first term of the MacLaurin expansion of
sin(n*phi - phi0) and sin(phi) and then making the division).
The original code had a 2PI term in the cosine (cos(2PI - nPhi))
but I removed it because it wasn't doing anything. AnaVV*/
sinmphi_sinphi = iaparams->p.overlap.para_c[ig]*
cos(iaparams->p.overlap.para_c[ig] * phi + iaparams->p.overlap.para_b[ig])/cosphi;
}
else {
sinmphi_sinphi = sin(iaparams->p.overlap.para_c[ig] * phi - iaparams->p.overlap.para_b[ig])/sin(phi);
}
fac *= sinmphi_sinphi;
f_dihe += fac;
}
/* store dihedral forces */
for(i=0;i<3;i++) {
force1[i] = f_dihe*v23Xf1[i];
force2[i] = f_dihe*(v34Xf4[i] - v12Xf1[i] - v23Xf1[i]);
force3[i] = f_dihe*(v12Xf1[i] - v23Xf4[i] - v34Xf4[i]);
}
return 0;
}
