Vector3 ImplicitSampler::construct_force(GridPoint _point, vector<GridPoint>* _neighborhood) {
Vector3 total_force(0,0,0);
Vector3 pt = _point.pt;
// TODO: precompute normals!
Vector3 normal = surface->normal(pt);
double total_weight = 0;
for(int n = 0; n < _neighborhood->size(); n++) {
Vector3 next_pt = _neighborhood->at(n).pt;
if(next_pt == pt)
continue;
Vector3 tangent_pt = next_pt + normal*((pt-next_pt).dotProduct(normal));
double dist = pt.length(next_pt);
double sin_weight_sqd = 1.0 / sin(pi_over_2*(dist/global_radius));
sin_weight_sqd *= sin_weight_sqd;
double weight = dist > global_radius ? 0 : (-pi_over_2*(1.0-sin_weight_sqd));
total_weight += weight;
Vector3 repulsion_vec = pt-tangent_pt;
total_force = total_force + repulsion_vec*weight;
}
total_force.scale(1.0/total_weight);
return total_force;
}
vector<rigid_sys::dynamic_info> rigid_sys::get_dynamic(number_t t,vector<dynamic_info> start)
{
vector<rigid_sys::dynamic_info> dynamic_dot;
for(int i=0;i<start.size();i++)
{
rigid_sys::dynamic_info change;
rigid_sys::dynamic_info origin;
origin=start[i];
//dx
change.x=origin.P/bodies[i].total_mass;
//dq
matrix3 R(origin.q);
matrix3 I_inv=R*bodies[i].Ibody_Inv*R.transpose();
vec omega=I_inv*origin.L;
change.q=quaternion(omega)*origin.q;
//dP
vec total_force(0,0,0);
for(int j=0;j<bodies[i].mass.size();j++)
total_force=total_force+extern_force(t,i,j);
change.P=total_force;
//dL
vec total_torque(0,0,0);
for(int j=0;j<bodies[i].mass.size();j++)
total_torque=total_torque+cross(matrix3(origin.q)*bodies[i].pos[j]-origin.x,extern_force(t,i,j));
change.L=total_torque;
dynamic_dot.push_back(change);
}
return dynamic_dot;
}
