void
efp_dipole_dipole_grad(const vec_t *d1, const vec_t *d2, const vec_t *dr,
vec_t *force, vec_t *add1, vec_t *add2)
{
double r = vec_len(dr);
double r3 = r * r * r;
double r5 = r3 * r * r;
double r7 = r5 * r * r;
double d1dr = vec_dot(d1, dr);
double d2dr = vec_dot(d2, dr);
double t1 = 3.0 / r5;
double t2 = t1 * vec_dot(d1, d2) - 15.0 / r7 * d1dr * d2dr;
force->x = t2 * dr->x + t1 * (d2dr * d1->x + d1dr * d2->x);
force->y = t2 * dr->y + t1 * (d2dr * d1->y + d1dr * d2->y);
force->z = t2 * dr->z + t1 * (d2dr * d1->z + d1dr * d2->z);
add1->x = d1->y * (d2->z / r3 - t1 * dr->z * d2dr) -
d1->z * (d2->y / r3 - t1 * dr->y * d2dr);
add1->y = d1->z * (d2->x / r3 - t1 * dr->x * d2dr) -
d1->x * (d2->z / r3 - t1 * dr->z * d2dr);
add1->z = d1->x * (d2->y / r3 - t1 * dr->y * d2dr) -
d1->y * (d2->x / r3 - t1 * dr->x * d2dr);
add2->x = d2->y * (d1->z / r3 - t1 * dr->z * d1dr) -
d2->z * (d1->y / r3 - t1 * dr->y * d1dr);
add2->y = d2->z * (d1->x / r3 - t1 * dr->x * d1dr) -
d2->x * (d1->z / r3 - t1 * dr->z * d1dr);
add2->z = d2->x * (d1->y / r3 - t1 * dr->y * d1dr) -
d2->y * (d1->x / r3 - t1 * dr->x * d1dr);
}
/* skew_midpoint() finds the middle of the minimal distance segment between
skew rays. Undefined for parallel rays.
Reference for algorithm:
docs/skew_midpoint.pdf
Arguments:
vec3d vert1, direct1 - vertex and direction unit vector of one ray.
vec3d vert2, direct2 - vertex and direction unit vector of other ray.
vec3d res - output buffer for midpoint result.
Returns:
The minimal distance between the skew lines.
*/
double skew_midpoint(vec3d vert1, vec3d direct1, vec3d vert2, vec3d direct2,
vec3d res)
{
vec3d perp_both, sp_diff, on1, on2, temp;
double scale;
/* vector between starting points */
vec_subt(vert2, vert1, sp_diff);
/* The shortest distance is on a line perpendicular to both rays. */
vec_cross(direct1, direct2, perp_both);
scale = vec_dot(perp_both, perp_both);
/* position along each ray */
vec_cross(sp_diff, direct2, temp);
vec_scalar_mul(direct1, vec_dot(perp_both, temp)/scale, temp);
vec_add(vert1, temp, on1);
vec_cross(sp_diff, direct1, temp);
vec_scalar_mul(direct2, vec_dot(perp_both, temp)/scale, temp);
vec_add(vert2, temp, on2);
/* Distance: */
scale = vec_diff_norm(on1, on2);
/* Average */
vec_add(on1, on2, res);
vec_scalar_mul(res, 0.5, res);
return scale;
}
/**
* m Model matrix
* x Return x in model space
* y Return y in model space
* p Mouse pointer at camera z plane
* dir Mouse pointer direction vector
*/
int
glw_widget_unproject(Mtx m, float *xp, float *yp,
const float *p, const float *dir)
{
Vector u, v, n, w0, T0, T1, T2, out, I;
float b, inv[16];
glw_mtx_mul_vec(T0, m, -1, -1, 0);
glw_mtx_mul_vec(T1, m, 1, -1, 0);
glw_mtx_mul_vec(T2, m, 1, 1, 0);
vec_sub(u, T1, T0);
vec_sub(v, T2, T0);
vec_cross(n, u, v);
vec_sub(w0, p, T0);
b = vec_dot(n, dir);
if(fabs(b) < 0.000001)
return 0;
vec_addmul(I, p, dir, -vec_dot(n, w0) / b);
if(!glw_mtx_invert(inv, m))
return 0;
glw_mtx_mul_vec(out, inv, I[0], I[1], I[2]);
*xp = out[0];
*yp = out[1];
return 1;
}
rgb lighting(scene s, ray r, hit_test h)
{
rgb result;
if (h.miss)
return s.bg;
vec hit_position = ray_position(r, h.dist);
if (shadow(hit_position, s.light, s.spheres)) {
result = rgb_modulate(h.surf, s.amb);
}
else {
double dot = vec_dot(h.surf_norm, s.light.direction);
double d = double_max(0, dot);
rgb diffuse_light = rgb_scale(d, s.light.color);
rgb lsum = rgb_add(s.amb, diffuse_light);
result = rgb_modulate(h.surf, lsum);
}
/**** === implement specular reflection here === ****/
if (rgb_nonzero(h.shine)) {
rgb ss;
vec N = h.surf_norm;
vec L = s.light.direction;
rgb S = h.shine;
vec R = vec_sub( vec_scale(2* vec_dot(N,L),N),L);
vec V = vec_neg(r.direction);
if (vec_dot(N,L)>0){
ss = rgb_scale( pow( double_max( vec_dot(R,V),0), 6), S);
//rgb_print(k);
}
else
ss = rgb_expr(0,0,0);
return rgb_add(result,ss);
}
return result;
}
double plane_t::hits(
vec_t *base, /* ray base */
vec_t *dir) /* unit direction vector */
{
double ndotd;
double t;
double ndotb;
ndotq = vec_dot(&normal, &point);
ndotd = vec_dot(dir, &normal);
/* ndotd = 0 -> ray is parallel to the plane */
if (ndotd == 0)
return(-1);
ndotb = vec_dot(&normal, base);
t = (ndotq - ndotb) / ndotd;
if (t <= 0)
return(-1);
vec_scale(t, dir, &hitloc);
vec_sum(&hitloc, base, &hitloc);
if (hitloc.z > 0.001)
return(-1);
return(t);
}
double plane_t::hits(vec_t *base, vec_t* dir){
double ndotd;
double t;
double ndotb;
ndotq = vec_dot(&normal, &point);
ndotd = vec_dot(dir, &normal);
/* ndotd = 0 -> ray is parallel to the plane */
if (ndotd == 0)
return(-1);
ndotb = vec_dot(&normal, base);
t = (ndotq - ndotb) / ndotd;
if (t <= 0)
return(-1);
vec_scale(t, dir, &last_hitpt);
vec_sum(&last_hitpt, base, &last_hitpt);
if ((last_hitpt.z > 0.01) && (strcmp(obj_type, "projector")))
return(-1);
return(t);
}
//Apply transform matrix to a vector
//v2 out
void vec_xform(mtx_t *m, vec_t *v1, vec_t *v2) {
vec_t v3;
v3.x = vec_dot(&(m->row[0]), v1);
v3.y = vec_dot(&(m->row[1]), v1);
v3.z = vec_dot(&(m->row[2]), v1);
*v2 = v3;
}
static void
get_induced_dipole_field(struct efp *efp, size_t frag_idx,
struct polarizable_pt *pt,
vec_t *field, vec_t *field_conj)
{
struct frag *fr_i = efp->frags + frag_idx;
*field = vec_zero;
*field_conj = vec_zero;
for (size_t j = 0; j < efp->n_frag; j++) {
if (j == frag_idx || efp_skip_frag_pair(efp, frag_idx, j))
continue;
struct frag *fr_j = efp->frags + j;
struct swf swf = efp_make_swf(efp, fr_i, fr_j);
for (size_t jj = 0; jj < fr_j->n_polarizable_pts; jj++) {
struct polarizable_pt *pt_j = fr_j->polarizable_pts + jj;
size_t idx = fr_j->polarizable_offset + jj;
vec_t dr = {
pt->x - pt_j->x + swf.cell.x,
pt->y - pt_j->y + swf.cell.y,
pt->z - pt_j->z + swf.cell.z
};
double r = vec_len(&dr);
double r3 = r * r * r;
double r5 = r3 * r * r;
double t1 = vec_dot(&efp->indip[idx], &dr);
double t2 = vec_dot(&efp->indipconj[idx], &dr);
double p1 = 1.0;
if (efp->opts.pol_damp == EFP_POL_DAMP_TT)
p1 = efp_get_pol_damp_tt(r, fr_i->pol_damp, fr_j->pol_damp);
field->x -= swf.swf * p1 * (efp->indip[idx].x / r3 -
3.0 * t1 * dr.x / r5);
field->y -= swf.swf * p1 * (efp->indip[idx].y / r3 -
3.0 * t1 * dr.y / r5);
field->z -= swf.swf * p1 * (efp->indip[idx].z / r3 -
3.0 * t1 * dr.z / r5);
field_conj->x -= swf.swf * p1 * (efp->indipconj[idx].x / r3 -
3.0 * t2 * dr.x / r5);
field_conj->y -= swf.swf * p1 * (efp->indipconj[idx].y / r3 -
3.0 * t2 * dr.y / r5);
field_conj->z -= swf.swf * p1 * (efp->indipconj[idx].z / r3 -
3.0 * t2 * dr.z / r5);
}
}
}
// Standard CG method starting from zero vector
// (because we solve for the increment)
// x... comes as right-hand side, leaves as solution
bool CommonSolverCG::_solve(Matrix* A, double *x, double tol, int maxiter)
{
printf("CG solver\n");
int n_dof = A->get_size();
double *r = new double[n_dof];
double *p = new double[n_dof];
double *help_vec = new double[n_dof];
if (r == NULL || p == NULL || help_vec == NULL) {
_error("a vector could not be allocated in solve_linear_system_iter().");
}
// r = b - A*x0 (where b is x and x0 = 0)
for (int i=0; i < n_dof; i++) r[i] = x[i];
// p = r
for (int i=0; i < n_dof; i++) p[i] = r[i];
// setting initial condition x = 0
for (int i=0; i < n_dof; i++) x[i] = 0;
// CG iteration
int iter_current = 0;
double tol_current;
while (1)
{
mat_dot(A, p, help_vec, n_dof);
double r_times_r = vec_dot(r, r, n_dof);
double alpha = r_times_r / vec_dot(p, help_vec, n_dof);
for (int i=0; i < n_dof; i++) {
x[i] += alpha*p[i];
r[i] -= alpha*help_vec[i];
}
double r_times_r_new = vec_dot(r, r, n_dof);
iter_current++;
tol_current = sqrt(r_times_r_new);
if (tol_current < tol
|| iter_current >= maxiter) break;
double beta = r_times_r_new/r_times_r;
for (int i=0; i < n_dof; i++) p[i] = r[i] + beta*p[i];
}
bool flag;
if (tol_current <= tol)
flag = true;
else
flag = false;
if (r != NULL) delete [] r;
if (p != NULL) delete [] p;
if (help_vec != NULL) delete [] help_vec;
printf("CG solver: maxiter: %i, tol: %e\n",
iter_current, tol_current);
return flag;
}
// Collision detection of two boxs with arbitrary angles.
// Does only one side (a->b) of the collision.
// -- If no collision, then no collision..
// -- If returns collision, check the other side (b<-a)
// -- A vector will be returned to displace 'a' so that there is no
// no collision anymore. The vector returned is one of the normals of 'b'.
static vec_t box_o_o_collision(box_t a, box_t b) {
// We will perform the collision using b's point of view and axes because
// those will be consided the surface normals, and we want to return 'a's'
// displacement in terms of 'a's' orientation vectors. -- So this may at times look reveresed.
vec_t dist = vec_diff(b.pos, a.pos);
// Normalized axis of box b. One of those axes will be the surface normal.
vec_t axn[2]; // = { b.axis0, b.axis1 };
float axl[2]; // Half axis length.
// Half axis.
vec_t ahx0 = vec_scale(a.axis0, a.size.x);
vec_t ahx1 = vec_scale(a.axis1, a.size.y);
// Distance we need to displace 'a' to remove the collision.
// -- (0, 0) => no collision.
float col_vec[2] = { 0.0, 0.0 };
int i = 2;
axn[0] = b.axis0;
axn[1] = b.axis1;
axl[0] = b.size.x;
axl[1] = b.size.y;
while(i--) {
float a1, a2, d;
// We check if the distance from center to center is smaller
// than the sum of the distance from center to box edge
// and that projection along each vector normal.
// If it is false for one normal, we have no collision.
// Otherwise we have the displacement distance to remove the collision.
a1 = vec_absdot(axn[i], ahx0);
a2 = vec_absdot(axn[i], ahx1);
d = vec_absdot(axn[i], dist);
col_vec[i] = a1 + a2 + axl[i] - d;
if(col_vec[i] <= 0) {
// Then there was no intersection.
return vec_new(0, 0);
}
}
if(col_vec[0] < col_vec[1]) {
if(vec_dot(dist, axn[0]) > 0) {
return vec_scale(axn[0], col_vec[0]);
} else {
return vec_scale(axn[0], -col_vec[0]);
}
} else {
if(vec_dot(dist, axn[1]) > 0) {
return vec_scale(axn[1], col_vec[1]);
} else {
return vec_scale(axn[1], -col_vec[1]);
}
}
}
/* Computes the phong hilight */
static void ga_shade_phong(vec_t *color, const ga_material_t *mat,const vec_t *lcolor, const vec_t *ldir, const vec_t *dir, const vec_t *norm, float factor){
vec_t r;
float fact;
float power;
r = vec_sub(
vec_scale(2.0f,vec_scale(vec_dot(*norm,*ldir),*norm)),
*ldir);
if((fact = vec_dot(r,vec_neg(*dir))) > 0.0f){
power = powf(fact,mat->spec_power)*mat->spec_factor;
*color = vec_add(*color, vec_scale( power*factor,
vec_mult(mat->spec_color,*lcolor)));
}
}
double
efp_dipole_dipole_energy(const vec_t *d1, const vec_t *d2, const vec_t *dr)
{
double r = vec_len(dr);
double r2 = r * r;
double r3 = r2 * r;
double r5 = r3 * r2;
double d1dr = vec_dot(d1, dr);
double d2dr = vec_dot(d2, dr);
return vec_dot(d1, d2) / r3 - 3.0 * d1dr * d2dr / r5;
}
COLOR_T illuminate (SCENE_T scene, RAY_T ray, VP_T int_pt, VP_T normal, int object) {
COLOR_T color;
COLOR_T input_color = scene.objs[object].color;
if(scene.objs[object].checkerboard) {
if(((int)floor(int_pt.x) + (int)floor(int_pt.y) + (int)floor(int_pt.z)) & 1) {
input_color = scene.objs[object].color2;
}
}
//ambient
color.R = 0.1 * scene.objs[object].color.R;
color.G = 0.1 * scene.objs[object].color.G;
color.B = 0.1 * scene.objs[object].color.B;
if(!test_shadow(scene, int_pt, normal, object)) {
//diffuse
VP_T L = vec_subtract(scene.light.location, int_pt);
double dL = vec_len(L);
L = vec_normalize(L);
double dot_L = vec_dot(normal, L);
double c1 = 0.002,
c2 = 0.02,
c3 = 0.2;
double light_atten = 1.0 / (c1 * dL * dL +
c2 * dL + c3);
if (dot_L > 0) {
color.R += dot_L * input_color.R * light_atten;
color.G += dot_L * input_color.G * light_atten;
color.B += dot_L * input_color.B * light_atten;
//specular
VP_T R;
R.x = L.x - 2 * dot_L * normal.x;
R.y = L.y - 2 * dot_L * normal.y;
R.z = L.z - 2 * dot_L * normal.z;
R = vec_normalize(R);
double dot_R = vec_dot(R, ray.direction);
if (dot_R > 0) {
color.R += pow(dot_R, 100);
color.G += pow(dot_R, 100);
color.B += pow(dot_R, 100);
}
}
}
return color;
}
t_hitpoint hitplane(t_ray *r, t_plane *p)
{
double t;
t_hitpoint h;
h.n = p->n;
t = -(p->d - vec_dot(h.n, r->o)) / vec_dot(h.n, r->dir);
if (t < EPSILON)
return(miss());
h.t = t;
h.p = vec_add(r->o, vec_scalar(r->dir, h.t));
h.c = p->color;
// h.m = p->m;
return (h);
}
void
lightsource_fake_phong (float *vertex, float *norm, const float *light_pos,
const float *light_up, const float *eye_pos,
matrix_t *invcamera, vertex_attrs *vertex_info,
unsigned int idx)
{
float norm_light[3];
float eye_to_vertex[3], tmp[3], reflection[3];
float light_to_vertex[3];
float eye_dot_norm;
vec_sub (light_to_vertex, light_pos, vertex);
vec_normalize (norm_light, light_to_vertex);
vec_sub (eye_to_vertex, eye_pos, vertex);
vec_normalize (eye_to_vertex, eye_to_vertex);
eye_dot_norm = vec_dot (eye_to_vertex, norm);
vec_scale (tmp, norm, 2.0 * eye_dot_norm);
vec_sub (reflection, tmp, eye_to_vertex);
if (eye_dot_norm < 0)
vertex_info[idx].fakephong.transformed_z = -0.01;
else
fakephong_texcoords (&vertex_info[idx].fakephong.texc[0],
&vertex_info[idx].fakephong.texc[1],
reflection, norm_light, light_up,
&vertex_info[idx].fakephong.transformed_z);
}
//Brief: QR decomposition
void qrdec(mat *A, mat* R){
double proj = 0;
double norm = 0;
// Allocates pointers of type *vec
//vec *coli = (vec*)malloc(sizeof(vec));
//vec *colj = (vec*)malloc(sizeof(vec));
vec* coli = vec_new(A->row);
vec* colj = vec_new(A->row);
for(int i=0; i<A->col; i++){
mat_get_col(coli, A, i);
norm = vec_norm(coli);
mat_set(R, i, i, norm);
vec_scale(coli, 1.0/norm);
mat_set_col(A, coli, i);
for(int j=i+1; j<A->col; j++){
mat_get_col(colj, A, j);
proj = vec_dot(coli,colj);
vec_add(colj,coli,-proj);
mat_set_col(A, colj, j);
mat_set(R, i, j, proj);
}
}
// Free pointers
vec_free(coli);
vec_free(colj);
}
Vector getReflectedRay(Vector surface_norm, Vector light_ray)
{
float cosTheta1 = vec_dot(surface_norm, vec_scale(light_ray,-1));
Vector reflect_ray = vec_plus(light_ray, vec_scale(surface_norm, 2*cosTheta1));
return reflect_ray;
}
Vector getRefractedRay(float initial_refraction, Spheres *sph, Vector surface_norm, Vector light_ray)
{
float refraction_index = sph->refraction_index;
float n = initial_refraction/refraction_index;
Vector refract_ray;
//n = .9;
//light_ray.x = 0.707107;
//light_ray.y = -0.707107;
//light_ray.z = 0;
//surface_norm.x = 0;
//surface_norm.y = 1;
//surface_norm.z = 0;
float cosTheta1 = vec_dot(surface_norm, vec_scale(light_ray,-1));
float cosTheta2 = sqrt(1.0f-pow(n,2)*(1-(cosTheta1*cosTheta1)));
Vector a = vec_scale(light_ray, n);
Vector b = vec_scale(surface_norm, n*cosTheta1 - cosTheta2);
if(cosTheta1 > 0.0f)
{
refract_ray = vec_plus(vec_scale(light_ray,n), vec_scale(surface_norm, n*cosTheta1-cosTheta2));
}
else
{
refract_ray = vec_minus(vec_scale(light_ray,n), vec_scale(surface_norm, n*cosTheta1-cosTheta2));
}
return refract_ray;
}
//project a vector onto a plane
void vec_project(vec_t *n, vec_t *v, vec_t *p) {
vec_t *r = (vec_t *)malloc(sizeof(vec_t));
vec_scale(vec_dot(n, v), n, r);
vec_diff(r, v, p);
free(r);
}
static int
classify_point(vec3_t p, int plane_n)
{
/* Determine which side of plane p is on */
plane_t *plane = &(g->r_planes[plane_n]);
return (vec_dot(p, plane->vec) < plane->offset ? -1 : 1);
}
/* diffuse shading */
static void ga_shade_diffuse(vec_t *color, const ga_material_t *mat,const vec_t *lcolor, const vec_t *ldir, const vec_t *norm, float factor){
float fact = vec_dot(*norm,*ldir);
if(fact > 0.0f){
*color = vec_add(*color,
vec_scale(fact*factor*mat->diff_factor
,vec_mult(mat->diff_color,*lcolor)));
}
}
double
efp_charge_dipole_energy(double q1, const vec_t *d2, const vec_t *dr)
{
double r = vec_len(dr);
double r3 = r * r * r;
return -q1 / r3 * vec_dot(d2, dr);
}
Float palRevoluteLink::GetAngularVelocity() const {
palVector3 av1,av2,axis;
palBody *pb =dynamic_cast<palBody *>(m_pParent);
palBody *cb =dynamic_cast<palBody *>(m_pChild);
vec_set(&av1,0,0,0);
vec_set(&av2,0,0,0);
if (pb)
pb->GetAngularVelocity(av1);
if (cb)
cb->GetAngularVelocity(av2);
axis = GetAxis();
Float rate;
rate =vec_dot(&axis,&av1);
rate-=vec_dot(&axis,&av2);
return rate;
}
vec_t vec_project(vec_t a,vec_t b){
float d = vec_dot(a,b);
float l = b.x*b.x + b.y*b.y;
if(l == 0.0){
return vec_new(0,0);
}else{
d /= l;
return vec_new(d*b.x, d*b.y);
}
}
// Returns the angle between the two vectors vec1 and vec2.
// Calculate angle as:
// a.b = |a||b|cos(theta)
double angle_btw_vec3(double vec1[3], double vec2[3])
{
double numer = vec_dot(vec1, vec2);
double denom = vec_dot(vec1, vec1) * vec_dot(vec2, vec2);
// v_print(vec1, 0);
// v_print(vec2, 0);
// printf("%.4f / %.4f\n", numer, sqrt(denom));
if (denom < ANGLE_TOLERANCE)
return 0.0;
// Delay taking the square-root
double angle = numer / sqrt(denom);
// printf("angle: %.4f, ", angle);
angle = acos(angle);
// printf(" %.4f <%.4f>\n", angle, angle*RAD2DEG);
return angle;
}
double lines_distance_3d(double a1[3], double a2[3],
double b1[3], double b2[3])
{
double cp1[3], cp2[3], len[3];
line_segment_closest_points(a1, a2, b1, b2, cp1, cp2);
vec_subt(len, cp2, cp1);
return vec_dot(len, len);
}
hit_test intersect(ray r, sphere s)
{
hit_test result = {0}; /* {0} to quiet the compiler */
vec sc = s.center;
double sr = s.radius;
vec A = vec_sub(r.origin, sc);
double B = vec_dot(A, r.direction);
double C = vec_dot(A, A) - (sr * sr);
double D = (B * B) - C;
double t = (-B - sqrt(D));
result.miss = 1;
if (D > 0 && t > 0) {
result.miss = 0;
result.dist = t;
result.surf = s.surf;
result.shine = s.shine;
result.surf_norm = vec_norm(vec_sub(ray_position(r,t),sc));
}
return result;
}
vector<pdi> ClusterPruning::get_k_nearest(const vector<double> &vec_id,int k){
vector<pdi> p;
for(auto &e:leader){
int leader_did=e.first;
auto vec_l=get_doc_vec(leader_did);
p.push_back(make_pair(vec_dot(vec_id,vec_l),leader_did));
}
nth_element(p.begin(),p.begin()+k-1,p.end());
p.erase(p.begin()+k,p.end());
return p;
}
//
// reflect vector p based on normalized vector n
//
Vector vec_reflect(Vector p, Vector n) {
Vector rc;
p.x = -(p.x);
p.y = -(p.y);
p.z = -(p.z);
normalize(&n);
rc = vec_minus(p ,vec_scale(n, (float)(2*(vec_dot(p, n)))));
return rc;
}
//reflect a vector from a surface plane]
void vec_reflect(vec_t *n, vec_t *w, vec_t *v) {
vec_t *u = (vec_t *)malloc(sizeof(vec_t));
vec_unit(w, u);
vec_scale(-1, u, u);
vec_scale((2*vec_dot(u, n)), n, v);
vec_diff(u, v, v);
free(u);
}
