double SinValue(const Vector3D& a, const Vector3D& b, const Vector3D& c)
{
Vector3D ba = b-a;
Vector3D ca = c-a;
Vector3D t= ba.cross(ca);
return t.norm()/(ba.norm()*ca.norm());
}
Quaternion(Vector3D v1, Vector3D v2) {
const double k = v2.norm() / v1.norm();
const Vector3D d1 = v1.direction();
const Vector3D d2 = v2.direction();
if ( (d1 + d2).norm() < PRECISION ) {
Vector3D n;
srand( (unsigned int) time(0));
do {
double x = (double) rand() / RAND_MAX;
double y = (double) rand() / RAND_MAX;
double z = (double) rand() / RAND_MAX;
Vector3D v = Vector3D(x, y, z);
n = v - v1.direction()*(v*v1)/v1.norm();
} while (n.norm() < PRECISION );
init( 0.0, n.direction() );
} else if ( (d1 - d2).norm() < PRECISION ) {
init( 1.0, Vector3D(0.0, 0.0, 0.0) );
} else {
double phi = acos( v1.direction()*v2.direction() );
Vector3D a = v1.cross(v2).direction();
assert(a.norm() > PRECISION);
double w = cos(phi/2) * sqrt(k);
Vector3D u = a * sin(phi/2) * sqrt(k);
init(w, u);
}
}
Vector3D Vertex::normal( void ) const
// TODO Returns an approximate unit normal at this vertex, computed by
// TODO taking the area-weighted average of the normals of neighboring
// TODO triangles, then normalizing.
{
// TODO Compute and return the area-weighted unit normal.
//no boundary polygon
HalfedgeCIter h = this->halfedge();
Vector3D nrm(0, 0, 0);
double totalarea = 0;
do
{
h = h->twin();
FaceCIter f = h->face();
if(!f->isBoundary())
{
VertexCIter v1 = h->vertex();
VertexCIter v2 = h->next()->twin()->vertex();
Vector3D out = cross(v1->position - position, v2->position - position);
nrm += out;
totalarea += out.norm();
}
h = h->next();
}
while(h != this->halfedge());
nrm /= totalarea;
nrm.normalize();
return nrm;
}
inline Vector3D grad_spiky_kernel(Vector3D r) {
double r_l = r.norm();
if (r_l >= H || r_l < EPS_D) {
return Vector3D();
}
return -3 * 4.774648292756860 * intpow<2>(H - r_l) * r / (intpow<6>(H) * r_l);
}
inline void clamp(Vector3D &p, Vector3D delta_p, BVHAccel *bvh) {
double l = delta_p.norm();
if (l <= EPS_D) {
return;
}
Vector3D d = delta_p / l;
// Viewing from plane x = -1.0, clip it as well
bool intersect = false;
if (d.z != 0.0) {
double pt = (1.0 - p.z) / d.z;
if (pt > 0.0 && pt < l) {
l = pt;
intersect = true;
}
}
// light is at y = 1.49.
// to avoid particles being stuck between light and ceiling, clip it
if (d.y != 0.0) {
double pt = (1.49 - p.y) / d.y;
if (pt > 0.0 && pt < l) {
l = pt;
intersect = true;
}
}
Ray r(p, d, 0.0, l);
if (bvh->intersect(r) || intersect) {
p += (r.max_t - EPS_D) * d;
} else {
p += delta_p;
}
p.x = max(-1.0 + EPS_D, min(1.0 - EPS_D, p.x));
p.y = max(0.0 + EPS_D, min(1.49 - EPS_D, p.y));
p.z = max(-1.0 + EPS_D, min(1.0 - EPS_D, p.z));
}
void computeConicalTexInfo(const Vector3D &pt_inter, const Vector3D &normal, const Vector3D &dPpdx, const Vector3D &dPpdy, float &u, float &v, Vector3D &dTdx, Vector3D &dTdy) const{
Vector3D ptOnCone = pt_inter - m_vertex;
// from cone --> to integrate
float da = ptOnCone.norm();
float dt = std::sqrt(m_radius*m_radius+m_height*m_height);
float radiusAtPt = da*m_radius/dt;
Vector3D ptOnConeBis(U.dot(ptOnCone), V.dot(ptOnCone), W.dot(ptOnCone));
ptOnCone[0] = ptOnConeBis[0];//ptOnCone[0]/radiusAtPt;
ptOnCone[1] = ptOnConeBis[1];//ptOnCone[1]/radiusAtPt;
// texture coordinates
u = (std::atan2(ptOnCone[1],ptOnCone[0])+M_PI)/(2.f*M_PI);
v = ((dt-da)/dt);
// Texture differential
dTdx[0] = (1.f/(2.f*M_PI)) * (1.f/(1.f + std::pow(ptOnCone[1]/ptOnCone[0], 2.f))) *
(
((dPpdx[1]*radiusAtPt - ptOnCone[1]*(m_radius/(dt*da)))/std::pow(radiusAtPt, 2.f)) * ptOnCone[0]
- ptOnCone[1]*((dPpdx[0]*radiusAtPt - ptOnCone[0]*(m_radius/(dt*da)))/std::pow(radiusAtPt, 2.f))
)/std::pow(ptOnCone[0], 2.f);
dTdy[0] = (1.f/(2.f*M_PI)) * (1.f/(1.f + std::pow(ptOnCone[1]/ptOnCone[0], 2.f))) *
(
((dPpdy[1]*radiusAtPt - ptOnCone[1]*(m_radius/(dt*da)))/std::pow(radiusAtPt, 2.f)) * ptOnCone[0]
- ptOnCone[1]*((dPpdy[0]*radiusAtPt - ptOnCone[0]*(m_radius/(dt*da)))/std::pow(radiusAtPt, 2.f))
)/std::pow(ptOnCone[0], 2.f);
float coef = (1.f / (dt*da));
dTdx[1] = coef * dPpdx.dot(ptOnCone);
dTdy[1] = coef * dPpdy.dot(ptOnCone);
}
Cone (const Vector3D ¢er_, const Vector3D& vertex_, float radius_) : m_vertex(vertex_), m_radius (radius_) {
m_axis = center_ - m_vertex;
m_height = m_axis.norm();
m_axis = m_axis.normalize();
m_costheta = std::cos(std::atan2(m_radius, m_height));
W = m_axis;
Vector3D::make_basis(U, V, W);
}
Spectrum PointLight::sample_L(const Vector3D& p, Vector3D* wi,
float* distToLight,
float* pdf) const {
Vector3D d = position - p;
*wi = d.unit();
*distToLight = d.norm();
*pdf = 1.0;
return radiance;
}
Spectrum AreaLight::sample_L(const Vector3D& p, Vector3D* wi,
float* distToLight, float* pdf) const {
Vector2D sample = sampler.get_sample() - Vector2D(0.5f, 0.5f);
Vector3D d = position + sample.x * dim_x + sample.y * dim_y - p;
*wi = d.unit();
*distToLight = d.norm();
*pdf = 1.0f / (dim_x.norm() * dim_y.norm());
return dot(d, direction) < 0 ? radiance : Spectrum();
};
// assign
void Plane::assign(const Point3D &p1, const Point3D &p2, const Point3D &p3){
Vector3D v1, v2;
v1.difference(p3, p2);
v2.difference(p1, p2);
Vector3D vC;
vC.cross(v1, v2);
vC.norm();
assign(vC, p1);
}
//vGetNormal() finds the gradient of the scalar field at a point
//This gradient can be used as a very accurate vertx normal for lighting calculations
Vector3D Particles::getVertexNormal(Vector3D &pos) {
double fX = pos[0];
double fY = pos[1];
double fZ = pos[2];
Vector3D n;
n.x = estimateDensityAt(Vector3D(fX-GRADIENT_EPS, fY, fZ)) - estimateDensityAt(Vector3D(fX+GRADIENT_EPS, fY, fZ));
n.y = estimateDensityAt(Vector3D(fX, fY-GRADIENT_EPS, fZ)) - estimateDensityAt(Vector3D(fX, fY+GRADIENT_EPS, fZ));
n.z = estimateDensityAt(Vector3D(fX, fY, fZ-GRADIENT_EPS)) - estimateDensityAt(Vector3D(fX, fY, fZ+GRADIENT_EPS));
if (n.norm() > 0) {
return n.unit();
}
return n;
}
bool insideSurfaceCrossing(const Point3D &pTest, const Surface &s, const SpacialHash &faceHash){
// check against bounding box
if (pTest.x < s.pMin.x || pTest.x > s.pMax.x ||
pTest.y < s.pMin.y || pTest.y > s.pMax.y ||
pTest.z < s.pMin.z || pTest.z > s.pMax.z)
return false;
// generate the random ray
Vector3D testRay;
testRay.x = rand()/(float)RAND_MAX;
testRay.y = rand()/(float)RAND_MAX;
testRay.z = rand()/(float)RAND_MAX;
testRay.norm();
// setup SEADS grid stepper
SpacialHash::Stepper stepper;
stepper.sh = &faceHash;
Array<int> *cell = stepper.discreteSetup(pTest, testRay);
// flags for which triangles have been tested
int numTri = s.triangles.getSize();
Array<bool> done;
done.resize(numTri);
done.clear();
// start walking and testing
int count = 0;
while(cell) {
// test the triangles
int numInCell = cell->getSize();
for (int l = 0; l < numInCell; l++){
int triN = cell->index(l);
if (!done.index(triN)){
done.index(triN) = true;
const Surface::Triangle *tri = &s.triangles.index(triN);
const Point3D *p0 = &s.vertices.index(tri->v[0]).p;
const Point3D *p1 = &s.vertices.index(tri->v[1]).p;
const Point3D *p2 = &s.vertices.index(tri->v[2]).p;
float t = -1;
if (intersectTriangle(pTest, testRay, &t, *p2, *p1, *p0) && t >= 0)
count++;
}
}
cell = stepper.discreteStep();
}
return (count%2) != 0;
}
void LangevinLeapfrogSwitchingIntegrator::doHalfKick() {
const unsigned int count = app->positions.size();
for (unsigned int i = 0; i < count; i++ ) {
const Vector3D diff = (app->positions)[i] - myCenterOfMass;
const Real distance = diff.norm();
const Real distSquared = distance * distance;
// switch
Real switchValue;
if(distSquared > myCutoff2) {
switchValue = 0.0;
}
else if(distSquared >= mySwitchOn2) {
const Real c2 = myCutoff2 - distSquared;
const Real c4 = c2 * (mySwitch2 + 2.0 * distSquared);
switchValue = mySwitch1 * c2 * c4;
} else {
switchValue = 1.0;
}
Real complimentSwitchValue = 1.0 - switchValue;
const Real myGamma = switchValue * myGammaInside + complimentSwitchValue * myGammaOutside;
const Real dt = getTimestep() * Constant::INV_TIMEFACTOR; // in fs
const Real fdt = ( 1.0 - exp( -0.5 * myGamma * dt ) ) / myGamma;
const Real vdt = exp(-0.5*myGamma*dt);
const Real ndt = sqrt( ( 1.0 - exp( -myGamma * dt ) ) / (2.0 * myGamma) );
const Real forceConstant = 2 * Constant::BOLTZMANN * myLangevinTemperature *
myGamma;
// Generate gaussian random numbers for each spatial direction
//force order of generation
Real rand1 = randomGaussianNumber(mySeed);
Real rand2 = randomGaussianNumber(mySeed);
Real rand3 = randomGaussianNumber(mySeed);
//into vector
Vector3D gaussRandCoord1(rand3, rand2, rand1);
Real mass = app->topology->atoms[i].scaledMass;
Real sqrtFCoverM = sqrt(forceConstant / mass);
// semi-update velocities
app->velocities[i] = app->velocities[i]*vdt
+(*myForces)[i] * fdt / mass
+gaussRandCoord1*sqrtFCoverM*ndt;
}
buildMolecularMomentum(&app->velocities, app->topology);
}
int main(int , char** )
{
for (int k = 0; k < 100000; ++k) {
// create a random rotation matrix by sampling a random 3d vector
// that will be used in axis-angle representation to create the matrix
Vector3D rotAxisAngle = Vector3D::Random();
rotAxisAngle += Vector3D::Random();
Eigen::AngleAxisd rotation(rotAxisAngle.norm(), rotAxisAngle.normalized());
Matrix3D Re = rotation.toRotationMatrix();
// our analytic function which we want to evaluate
Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
// compute the Jacobian using AD
Matrix<double, 3, 9, Eigen::RowMajor> dq_dR_AD;
typedef ceres::internal::AutoDiff<RotationMatrix2QuaternionManifold, double, 9> AutoDiff_Dq_DR;
double *parameters[] = { Re.data() };
double *jacobians[] = { dq_dR_AD.data() };
double value[3];
RotationMatrix2QuaternionManifold rot2quat;
AutoDiff_Dq_DR::Differentiate(rot2quat, parameters, 3, value, jacobians);
// compare the two Jacobians
const double allowedDifference = 1e-6;
for (int i = 0; i < dq_dR.rows(); ++i) {
for (int j = 0; j < dq_dR.cols(); ++j) {
double d = fabs(dq_dR_AD(i,j) - dq_dR(i,j));
if (d > allowedDifference) {
cerr << "\ndetected difference in the Jacobians" << endl;
cerr << PVAR(Re) << endl << endl;
cerr << PVAR(dq_dR_AD) << endl << endl;
cerr << PVAR(dq_dR) << endl << endl;
return 1;
}
}
}
cerr << "+";
}
return 0;
}
void CObjectView::OnMouseMove(UINT nFlags, CPoint point)
{
if (GetCapture() == this && (nFlags & MK_LBUTTON)){
if (mode == PAN){
Point2D pL, pC;
screenToFrustum(&pL, lastPt);
screenToFrustum(&pC, point);
// update translate
tr[0] += (pC.x - pL.x)*1000.0/scale;
tr[1] += (pC.y - pL.y)*1000.0/scale;
}
else if (mode == ROTATE || mode == LIGHT){
Vector3D vL, vC;
screenToVector(&vL, lastPt);
screenToVector(&vC, point);
// calculate angle prop to length mouse movement
float dX = vC.x - vL.x;
float dY = vC.y - vL.y;
float dZ = vC.z - vL.z;
float ang = 90.0 * sqrt(dX*dX + dY*dY + dZ*dZ);
// vector is just cross product
Vector3D v;
v.cross(vL, vC);
v.norm();
if (mode == ROTATE)
applyRotation(m, ang, v);
else
applyRotation(mL, ang, v);
}
lastPt = point;
RedrawWindow();
}
CView ::OnMouseMove(nFlags, point);
}
bool insideSurfaceClosest(const Point3D &pTest, const Surface &s, const SpacialHash &faceHash, ClosestPointInfo *inf, float stopBelow, bool allowQuickTest){
if (inf)
inf->type = DIST_TYPE_INVALID;
// quick bounding box test
if (allowQuickTest){
if (pTest.x < s.pMin.x || pTest.x > s.pMax.x ||
pTest.y < s.pMin.y || pTest.y > s.pMax.y ||
pTest.z < s.pMin.z || pTest.z > s.pMax.z){
return false;
}
}
ClosestPointInfo localClosestInf;
if (!inf)
inf = &localClosestInf;
float dist = getClosestPoint(inf, pTest, s, faceHash, stopBelow);
if (dist < stopBelow){
// optimise for dodec
return true;
}
// vector to point on surface
Vector3D v;
v.difference(pTest, inf->pClose);
v.norm();
if (inf->type == FACE){
// face test
Vector3D n;
s.getTriangleNormal(&n, inf->triangle);
double dt = n.dot(v);
return dt <= 0;
}
else if (inf->type == EDGE){
// edge test
const Surface::Triangle *tri = &s.triangles.index(inf->triangle);
// edge will be between vertices v[num] and v[(num+1)%3]
int e[2];
e[0] = tri->v[inf->num];
e[1] = tri->v[(inf->num+1)%3];
int neigh = findNeighbour(s, *tri, e);
if (neigh >= 0){
// make a plane for one of the triangles
Vector3D n1;
s.getTriangleNormal(&n1, inf->triangle);
Point3D p1 = s.vertices.index(e[0]).p;
Plane pl1;
pl1.assign(n1, p1);
// get the point from the other triangle which is not part of edge
const Surface::Triangle *tri2 = &s.triangles.index(neigh);
for (int i = 0; i < 3; i++){
if (tri2->v[i] != e[0] && tri2->v[i] != e[1])
break;
}
CHECK_DEBUG0(i != 3);
Point3D p2 = s.vertices.index(e[1]).p;
// get signed distance to plane
float dist = pl1.dist(p2);
// need normal for second triangle
Vector3D n2;
s.getTriangleNormal(&n2, neigh);
if (dist <= 0.0f){
// faces form convex spike, back facing to both
return v.dot(n1) <= 0 && v.dot(n2) <= 0;
}
else{
// faces form concavity, back facing to either
return v.dot(n1) <= 0 || v.dot(n2) <= 0;
}
}
else{
OUTPUTINFO("HHHHHMMMMMMM loose edge\n");
return false; // only one triangle on edge - use face ??
}
}
else{// if (minType == VERTEX)
// chosen triangle
const Surface::Triangle *tri = &s.triangles.index(inf->triangle);
// chosen vertex
int vI = tri->v[inf->num];
Vector3D n;
s.getVertexNormal(&n, vI);
return n.dot(v) <= 0;
/*
// get all faces
Array<int> tris;
s.findNeighbours(&tris, vI, inf->triangle);
// behind test for all faces
int numTri = tris.getSize();
for (int i = 0; i < numTri; i++){
Vector3D n;
s.getTriangleNormal(&n, tris.index(i));
double dt = n.dot(v);
if (dt > 0)
return false;
}
// must be behind all
return true;*/
}
}
void CObjectView::OnDraw(CDC* pDC)
{
SELECT_CONTEXT();
setupPerspective();
drawScene(&dList, tex);
// turn off lighting
glDisable(GL_LIGHTING);
// draw tracker ball
if ((mode == ROTATE || mode == LIGHT)&& GetCapture() == this){
// do overlay
glEnable(GL_CULL_FACE);
glClear(GL_DEPTH_BUFFER_BIT);
glDisable(GL_STENCIL_TEST);
// work out size of tracker
float rad = 0.85f;
// setup transform
glMatrixMode(GL_MODELVIEW);
if (mode == LIGHT)
glLoadMatrixf(mL);
else
glLoadMatrixf(m);
// mesh
glPolygonMode(GL_FRONT, GL_LINE);
glLineWidth(1);
// draw light
if (mode == LIGHT){
// get position
Vector3D v = {1.0f, 1.0f, 1.0f};
v.norm();
v.scale(rad);
// setup dot size
GLint dotSize;
glGetIntegerv(GL_POINT_SIZE, &dotSize);
glPointSize(3);
// draw light
glColor3f(1.0f, 1.0f, 0.0f);
glBegin(GL_POINTS);
glVertex3f(v.x, v.y, v.z);
glEnd();
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(v.x, v.y, v.z);
glEnd();
// restore dot size
glPointSize(dotSize);
}
// draw sphere
glColor3f(1, 0, 1);
glRotatef(90, 1, 0, 0);
GLUquadricObj *q = gluNewQuadric();
gluSphere(q, rad, 10, 10);
gluDeleteQuadric(q);
}
// clean up and show picture
glFlush();
SwapBuffers(hDC);
UNSELECT_CONTEXT();
}
void LangevinFlowCoupledIntegrator::doHalfKick() {
const unsigned int count = app->positions.size();
const Real dt = getTimestep();// * Constant::INV_TIMEFACTOR; // in fs
//variables for diagnostics
Real average_velocity = 0.0;
Real ke = 0.0;
for (unsigned int i = 0; i < count; i++ ) {
//user defined fixed velocity
Vector3D fluidVelocity(
slopeVelocityX * app->positions[i][0] + averageVelocityX,
slopeVelocityY * app->positions[i][1] + averageVelocityY,
slopeVelocityZ * app->positions[i][2] + averageVelocityZ);
//find damping factor relative to the
//vector connecting thr SCE to the cell center
const unsigned int myCellCenter = cellCenters[app->topology->atoms[i].residue_seq];
const Real normFluidV = fluidVelocity.norm();
Real factor = 1.0;
//dont use velocity if center
if(myCellCenter != i && normFluidV != 0.0){
Vector3D sc = app->positions[myCellCenter] - app->positions[i];
//app->topology->minimalDifference( app->positions[i], app->positions[myCellCenter]);
factor = sc.dot(fluidVelocity) / ( normFluidV * sc.norm() );
//if(factor > 1.0) report << hint << "factor too big " << factor << endr;
if(factor < 0.0){
factor = 0.1;
}
}
//if cell center
if( myCellCenter == i ) factor = 0.1;
//factor must be +ve here, so scale gamma
Real aGamma = myGamma;// * factor; //####Removed factor
//Langevin leapfrog from here
const Real fdt = ( 1.0 - exp( -0.5 * aGamma * dt ) ) / aGamma;
const Real vdt = exp(-0.5*aGamma*dt);
const Real ndt = sqrt( ( 1.0 - exp( -aGamma * dt ) ) / (2.0 * aGamma) );
const Real forceConstant = 2 * Constant::SI::BOLTZMANN * 1.0e15 //for SI units, BOLTZMANN //
* myLangevinTemperature
* aGamma;
// Generate gaussian random numbers for each spatial direction
Vector3D gaussRandCoord1(randomGaussianNumber(mySeed),
randomGaussianNumber(mySeed),
randomGaussianNumber(mySeed));
Real mass = app->topology->atoms[i].scaledMass;
Real sqrtFCoverM = sqrt(forceConstant / mass);
//remove velocity
app->velocities[i] -= fluidVelocity;
// semi-update velocities
app->velocities[i] = app->velocities[i]*vdt
+(*myForces)[i] * fdt / mass
//+ projectedVelocity * fdt
+gaussRandCoord1*sqrtFCoverM*ndt;
//+gaussRandCoord1*(sqrt(sqrtFCoverM + sqrtFCoverMf))*ndt;
//find "real" temperature
for(int k=0; k<3; k++){
ke += 0.5 * app->velocities[i].c[k] * app->velocities[i].c[k] * mass;
}
//replace velocity
app->velocities[i] += fluidVelocity;
//find average for diagnostics
average_velocity += app->velocities[i].c[0];
}
//diagnostic output
//report << hint << "Average velocity " << average_velocity / (Real)count
// << " Set velocity " << averageVelocityX << " Temp " << 2.0 * ke / Constant::BOLTZMANN / count / 3.0 << endr;
buildMolecularMomentum(&app->velocities, app->topology);
}
