bool SATChecker::satAlgorithm(Vec* norms, size_t normalsCount,
Vec* shapeAPoints, Vec shapeBPosition, double shapeBRadius) {
for (size_t i = 0; i < normalsCount; i++) {
Vec max(0), min(0);
double maxNormA = 0.0, minNormA = 0.0;
for (size_t j = 0; j < normalsCount; j++) {
Vec proy = (shapeAPoints[j]).proyected(norms[i]);
double proyNorm = proy.norm();
if (j == 0 || proyNorm < minNormA) {
min = proy;
minNormA = proyNorm;
}
if (j == 0 || proy.norm() > max.norm()) {
max = proy;
maxNormA = proyNorm;
}
}
Vec proyC = shapeBPosition.proyected(norms[i]);
double proyNorm = proyC.norm();
if ((proyNorm - shapeBRadius > maxNormA) || (proyNorm + shapeBRadius < minNormA))
return true;
}
return false;
}
//x is point on the object; y is point on the light
float PathTracerSplitted::radianceTransfer(const Point3D &x, const Point3D &y ) {
Vec vecx = Vec(x.x,x.y,x.z),vecy = Vec(y.x,y.y,y.z);
Vec normy = y.obj->getNorm(y);
//
Vec vecyx = vecx - vecy;
vecyx.norm();
//Escape when the light is same direction
float consine2 = Dot(vecyx,normy);
if(consine2<0)
return 0;
Vec vecxy = vecy - vecx;
vecxy.norm();
Vec normx = x.obj->getNorm(x);
float consine1 = Dot(vecxy,normx);
if(consine1<0)
return 0;
if(!visibility(x,y))
return 0;
float r = (x-y).length();
//shortDis = shortDis < r? shortDis:r;//???????????????????
//dis.push_back(r);
return consine1*consine2/(r*r);
};
/**
* @brief Check whether the given position inside this region is acceptable in a
* Monte Carlo rejection sampling of the density field for the dark matter
*
* @param position Position inside the region
* @return True if the position is accepted, false if it is rejected
*/
bool ICRegion::accept_dm(Vec position) {
Vec p = position - _origin;
#if ndim_ == 3
return ((double)rand()) / ((double)RAND_MAX) <
((*_dmfunction[0])(p.norm(), p.x(), p.y(), p.z())) / _max_value_dm;
#else
return ((double)rand()) / ((double)RAND_MAX) <
((*_dmfunction[0])(p.norm(), p.x(), p.y())) / _max_value_dm;
#endif
}
Viewer::Vec
Viewer::next_around_circle(const float& phi, const Vec& pos, const Vec& ori) {
Vec cam = pos-ori;
Vec cam_norm = cam/cam.norm();
Vec y(cam_norm.z, 0, -cam_norm.x);
Vec y_norm = y/y.norm();
Vec new_cam = ori + (cam_norm*cos(phi) + y_norm*sin(phi)) * cam.norm();
return new_cam;
}
void
DGtal::Viewer3D::glDrawGLPointel ( pointD3D pointel )
{
if ( !pointel.isSigned )
{
glPushMatrix();
glTranslatef ( pointel.x, pointel.y, pointel.z );
GLUquadric* quadric = gluNewQuadric();
glColor4ub ( pointel.R, pointel.G, pointel.B, pointel.T );
gluSphere ( quadric, pointel.size, 10, 10 );
glPopMatrix();
}
else
{
// a small "+" is drawn with cylinder
if ( pointel.signPos )
{
glPushMatrix();
glTranslatef ( pointel.x-0.07, pointel.y-0.07, pointel.z );
Vec dir ( 0.14, 0.14, 0 );
glMultMatrixd ( Quaternion ( Vec ( 0,0,1 ), dir ).matrix() );
GLUquadric* quadric = gluNewQuadric();
glColor4ub ( pointel.R, pointel.G, pointel.B, pointel.T );
gluCylinder ( quadric, pointel.size/3.0 , pointel.size/3.0,
dir.norm(),10, 4 );
glPopMatrix();
glPushMatrix();
glTranslatef ( pointel.x-0.07, pointel.y+0.07, pointel.z );
dir=Vec ( 0.14, -0.14, 0 );
glMultMatrixd ( Quaternion ( Vec ( 0,0,1 ), dir ).matrix() );
quadric = gluNewQuadric();
glColor4ub ( pointel.R, pointel.G, pointel.B, pointel.T );
gluCylinder ( quadric, pointel.size/3.0 , pointel.size/3.0,
dir.norm(),10, 4 );
glPopMatrix();
}
else
{
glPushMatrix();
glTranslatef ( pointel.x, pointel.y+0.07, pointel.z-0.07 );
Vec dir ( 0.0, -0.14, 0.14 );
glMultMatrixd ( Quaternion ( Vec ( 0,0,1 ), dir ).matrix() );
GLUquadric* quadric = gluNewQuadric();
glColor4ub ( pointel.R, pointel.G, pointel.B, pointel.T );
gluCylinder ( quadric, pointel.size/4.0 , pointel.size/4.0,
dir.norm(),10, 4 );
glPopMatrix();
}
}
}
int main() {
double y_vals[] = {-1.5, 2, -2.5};
double z_vals[] = {3, -2, 1};
Vec<double> zeroes(3); // Vec size 3 (entries initialize to zero)
Vec<double> x = Vec<double>::constantVec(3, 2.5); // Vec size 3 with all entries set to 2.5
Vec<double> y = Vec<double>(y_vals, 3);
Vec<double> z(3);
z.setEntries(z_vals, 3);
Vec<int> ix(x);
cout << "zeroes = " << zeroes << endl;
cout << "x = " << x << endl;
cout << "y = " << y << endl;
cout << "z = " << z << endl;
cout << "ix = " << ix << endl;
cout << "z[0] = " << z[0] << ", z[1] = " << z[1] << ", z[2] = " << z[2] << endl;
cout << "3.5 * x = " << (3.5 * x) << endl;
cout << "x / 3.5 = " << (x / 3.5) << endl;
cout << "x + y = " << (x + y) << endl;
cout << "x - y = " << (x - y) << endl;
cout << "x.concatenate(y) = " << x.concatenate(y) << endl;
cout << "x.dot(y) = " << x.dot(y) << endl;
cout << "x.cross(y) = " << x.cross(y) << endl;
cout << "x.norm() = " << x.norm() << endl;
cout << "x.unit_vector() = " << x.unit_vector() << endl;
cout << "ix.norm() = " << ix.norm() << endl;
cout << "ix.norm<double>() = " << ix.norm<double>() << endl;
cout << "ix.unit_vector<double>() = " << ix.unit_vector<double>() << endl;
cout << "scalar_triple_product(x, y, z) = "
<< Vec<double>::scalar_triple_product(x, y, z) << endl;
cout << "vector_triple_product(x, y, z) = "
<< Vec<double>::vector_triple_product(x, y, z) << endl;
}
int main(int argc, char *argv[]){
int w=1024, h=768, samps = argc==2 ? atoi(argv[1])/4 : 1; // # samples
Ray cam(Vec(50,52,295.6), Vec(0,-0.042612,-1).norm()); // cam pos, dir
Vec cx=Vec(w*.5135/h), cy=(cx%cam.d).norm()*.5135, r, *c=new Vec[w*h];
#pragma omp parallel for schedule(dynamic, 1) private(r) // OpenMP
for (int y=0; y<h; y++){ // Loop over image rows
// *** Commented out for Visual Studio, fprintf is not thread-safe
//fprintf(stderr,"\rRendering (%d spp) %5.2f%%",samps*4,100.*y/(h-1));
unsigned short Xi[3]={0,0,y*y*y}; // *** Moved outside for VS2012
for (unsigned short x=0; x<w; x++) // Loop cols
for (int sy=0, i=(h-y-1)*w+x; sy<2; sy++) // 2x2 subpixel rows
for (int sx=0; sx<2; sx++, r=Vec()){ // 2x2 subpixel cols
for (int s=0; s<samps; s++){
double r1=2*erand48(Xi), dx=r1<1 ? sqrt(r1)-1: 1-sqrt(2-r1);
double r2=2*erand48(Xi), dy=r2<1 ? sqrt(r2)-1: 1-sqrt(2-r2);
Vec d = cx*( ( (sx+.5 + dx)/2 + x)/w - .5) +
cy*( ( (sy+.5 + dy)/2 + y)/h - .5) + cam.d;
r = r + radiance(Ray(cam.o+d*140,d.norm()),0,Xi)*(1./samps);
} // Camera rays are pushed ^^^^^ forward to start in interior
c[i] = c[i] + Vec(clamp(r.x),clamp(r.y),clamp(r.z))*.25;
}
}
FILE *f = fopen("image.ppm", "w"); // Write image to PPM file.
fprintf(f, "P3\n%d %d\n%d\n", w, h, 255);
for (int i=0; i<w*h; i++)
fprintf(f,"%d %d %d ", toInt(c[i].x), toInt(c[i].y), toInt(c[i].z));
}
double SuperpositionIonicDensities::density(Vec const& position) const
{
double r(position.norm()*Constants::AngstromToBohr);
double f1(0.0), f2(0.0), d1(0.0), d2(0.0);
double q(std::abs(m_charge));
unsigned index (r/s_stepSize);
if (index < m_nNeutralData-1) {
f1 = (1.0-q) * m_neutralData[2*index];
d1 = (1.0-q) * m_neutralData[2*index+1];
f2 = (1.0-q) * m_neutralData[2*index+2];
d2 = (1.0-q) * m_neutralData[2*index+3];
}
if (index < m_nChargedData-1) {
f1 += q * m_chargedData[2*index];
d1 += q * m_chargedData[2*index+1];
f2 += q * m_chargedData[2*index+2];
d2 += q * m_chargedData[2*index+3];
}
r = (r-index*s_stepSize) / s_stepSize;
return (1.0-r)*f1 + r*f2;
// cubic interpolation gets messed up with the sudden changes at the origin
if (index < 2) return (1.0-r)*f1 + r*f2;
// cubic spline interpolation
double a( d1*s_stepSize - (f2-f1));
double b(-d2*s_stepSize + (f2-f1));
return (1.0-r)*f1 + r*f2 + r*(1.0-r) * (a*(1.0-r)+b*r);
}
/*! Sets the Quaternion from the three rotated vectors of an orthogonal basis.
The three vectors do not have to be normalized but must be orthogonal and direct (X^Y=k*Z, with k>0).
\code
Quaternion q;
q.setFromRotatedBasis(X, Y, Z);
// Now q.rotate(Vec(1,0,0)) == X and q.inverseRotate(X) == Vec(1,0,0)
// Same goes for Y and Z with Vec(0,1,0) and Vec(0,0,1).
\endcode
See also setFromRotationMatrix() and Quaternion(const Vec&, const Vec&). */
void Quaternion::setFromRotatedBasis(const Vec& X, const Vec& Y, const Vec& Z)
{
qreal m[3][3];
qreal normX = X.norm();
qreal normY = Y.norm();
qreal normZ = Z.norm();
for (int i=0; i<3; ++i)
{
m[i][0] = X[i] / normX;
m[i][1] = Y[i] / normY;
m[i][2] = Z[i] / normZ;
}
setFromRotationMatrix(m);
}
Vec Triangle::getNorm(Vec x) {
Vec a = p2-p1;
Vec b = p3-p1;
Vec result = Cross(a,b);
result.norm();
return result;
};
/*! Returns the normalized axis direction of the rotation represented by the Quaternion.
It is null for an identity Quaternion. See also angle() and getAxisAngle(). */
Vec Quaternion::axis() const
{
Vec res = Vec(q[0], q[1], q[2]);
const qreal sinus = res.norm();
if (sinus > 1E-8)
res /= sinus;
return (acos(q[3]) <= M_PI/2.0) ? res : -res;
}
float Cylindre::quantityIntersected(const qglviewer::Vec& _depart, const qglviewer::Vec& _arrivee, float _light_radius) const
{
// On va construire un cylindre de taille br=br+rlr/2, tr = tr+rlr/2, h = h +rlr/2
// on va créer un rayon d'origine depart et de direction arrivee - depart
// et vérifier si ce rayon intersecte les cylindres
float penombre;
Cylindre cylindre_penombre;
cylindre_penombre.setTopRadius(topradius()+_light_radius/2);
cylindre_penombre.setBottomRadius(bottomradius()+_light_radius/2);
cylindre_penombre.setHeight(height()+_light_radius/2);
Disque* disque_top = new Disque(cylindre_penombre.topradius());
Disque* disque_bottom = new Disque(cylindre_penombre.bottomradius());
disque_bottom->setMaterial(cylindre_penombre.material());
disque_top->setMaterial(cylindre_penombre.material());
Frame* frame_topdisque = new Frame();
*frame_topdisque = cylindre_penombre.frame();
frame_topdisque->setPosition(frame_topdisque->position()+Vec(0.0,0.0,cylindre_penombre.height()));
disque_top->setFrame(*frame_topdisque);
disque_bottom->setFrame(frame());
cylindre_penombre.setBottomDisque(disque_bottom);
cylindre_penombre.setTopDisque(disque_top);
Ray ray;
Vec dir = (_arrivee-_depart);
dir = dir / (dir.norm());
ray.setStart(_depart);
ray.setDirection(dir);
Hit hit;
if (this->intersect(ray,hit))
{
penombre = 1;
}
else
{
if (cylindre_penombre.intersect(ray,hit))
{
Vec I = hit.intersection();
I = cylindre_penombre.frame().coordinatesOf(I);
penombre = I.z/cylindre_penombre.height();
}
else
{
penombre = 0;
}
}
return penombre;
}
/*! Projects the Vec on the axis of direction \p direction that passes through the origin.
\p direction does not need to be normalized (but must be non null). */
void Vec::projectOnAxis(const Vec& direction)
{
#ifndef QT_NO_DEBUG
if (direction.squaredNorm() < 1.0E-10)
qWarning("Vec::projectOnAxis: axis direction is not normalized (norm=%f).", direction.norm());
#endif
*this = (((*this)*direction) / direction.squaredNorm()) * direction;
}
/*! Projects the Vec on the plane whose normal is \p normal that passes through the origin.
\p normal does not need to be normalized (but must be non null). */
void Vec::projectOnPlane(const Vec& normal)
{
#ifndef QT_NO_DEBUG
if (normal.squaredNorm() < 1.0E-10)
qWarning("Vec::projectOnPlane: plane normal is not normalized (norm=%f).", normal.norm());
#endif
*this -= (((*this)*normal) / normal.squaredNorm()) * normal;
}
double MultipolePotential::potential(double const x, double const y, double const z) const
{
double esp(0.0);
double tmp, R2, s, ir1, ir2, ir3, ir5, ir7;
Vec pos(x, y, z);
Vec R;
Data::MultipoleExpansionList::const_iterator site;
for (site = m_siteList.begin(); site != m_siteList.end(); ++site) {
R = pos-(*site)->position();
R *= Constants::AngstromToBohr;
R2 = R.squaredNorm();
ir1 = 1.0/R.norm();
ir2 = ir1*ir1;
ir3 = ir1*ir2;
ir5 = ir3*ir2;
ir7 = ir5*ir2;
if (m_order >= 0) { // charge
esp += (*site)->moment(Data::MultipoleExpansion::Q) * ir1;
}
if (m_order >= 1) { // dipole
tmp = (*site)->moment(Data::MultipoleExpansion::X) * R.x;
tmp += (*site)->moment(Data::MultipoleExpansion::Y) * R.y;
tmp += (*site)->moment(Data::MultipoleExpansion::Z) * R.z;
esp += tmp * ir3;
}
if (m_order >= 2) { // quadrupole
tmp = (*site)->moment(Data::MultipoleExpansion::XX) * (3.0*R.x*R.x - R2);
tmp += (*site)->moment(Data::MultipoleExpansion::YY) * (3.0*R.y*R.y - R2);
tmp += (*site)->moment(Data::MultipoleExpansion::ZZ) * (3.0*R.z*R.z - R2);
tmp += (*site)->moment(Data::MultipoleExpansion::XY) * (3.0*R.x*R.y);
tmp += (*site)->moment(Data::MultipoleExpansion::XZ) * (3.0*R.x*R.z);
tmp += (*site)->moment(Data::MultipoleExpansion::YZ) * (3.0*R.y*R.z);
esp += 0.5*tmp*ir5;
}
if (m_order >= 3) { // octopole
tmp = (*site)->moment(Data::MultipoleExpansion::XYZ) * (30.0*R.x*R.y*R.z);
s = 5.0*R.x*R.x;
tmp += (*site)->moment(Data::MultipoleExpansion::XXX) * R.x*(s - 3.0*R2);
tmp += (*site)->moment(Data::MultipoleExpansion::XXY) * 3.0*R.y*(s - R2);
tmp += (*site)->moment(Data::MultipoleExpansion::XXZ) * 3.0*R.z*(s - R2);
s = 5.0*R.y*R.y;
tmp += (*site)->moment(Data::MultipoleExpansion::XYY) * 3.0*R.x*(s - R2);
tmp += (*site)->moment(Data::MultipoleExpansion::YYY) * R.y*(s - 3.0*R2);
tmp += (*site)->moment(Data::MultipoleExpansion::YYZ) * 3.0*R.z*(s - R2);
s = 5.0*R.z*R.z;
tmp += (*site)->moment(Data::MultipoleExpansion::XZZ) * 3.0*R.x*(s - R2);
tmp += (*site)->moment(Data::MultipoleExpansion::YZZ) * 3.0*R.y*(s - R2);
tmp += (*site)->moment(Data::MultipoleExpansion::ZZZ) * R.z*(s - 3.0*R2);
esp += 0.5*tmp*ir7;
}
}
return esp;
}
/// spit out a vector of points
Watt& operator()(){
vp.clear();
ap.clear();
bp.clear();
fitness = 0;
length = 0;
Point pa = Round::point(-distance/2,0,0);
Point pb = Round::point(distance/2,0,0);
Dls sa = Round::dls(pa,distance*ra);
Dls sb = Round::dls(pb,distance*rb);
Cir ca = (sa^Dlp(0,0,1)).dual();
auto cb = (sb^Dlp(0,0,1));
//samples
for (int i=0;i<50;++i){
double theta = PI * i/50;
auto tp = Round::pnt_cir(ca, theta);
Dls sc = Round::dls(tp, rc*distance);
//intersection
auto meet = (sc ^ sb).dual();
Vec tmid; Vec last;
if (Round::size(meet, false) >= 0){
auto par = ( meet.dual() ^ Dlp(0,0,1) ).dual();
auto tq = Round::loc( Round::split(par,true));
auto mid = ( tp + ( (tq - tp ) *.5) ).null();
vp.push_back(mid);
ap.push_back(tp);
bp.push_back(tq);
//spread
tmid += Vec(mid) - last;
last = mid;
//collinearity
// auto pln = mid ^ LN(0,1,0);
// fitness += fabs(pln.rnorm());
}
Lin ln;
if (!vp.empty()){
ln = vp[0] ^ vp[ vp.size()-1] ^ Inf(1);
for (auto& i : vp){
auto pln = i ^ ln;
fitness += fabs(pln.rnorm());
}
}
length = fabs(tmid.norm());
}
return *this;
}
Vec Face::normal() const
{
Vec n = (*v[1] - *v[0]) ^ (*v[2] - *v[0]);
double length = n.norm();
if(length < 1.0E-10)
return n;
else
return n /= length;
}
void ManipulatedCameraFrame::zoom(qreal delta, const Camera * const camera) {
const qreal sceneRadius = camera->sceneRadius();
if (zoomsOnPivotPoint_) {
Vec direction = position() - camera->pivotPoint();
if (direction.norm() > 0.02 * sceneRadius || delta > 0.0)
translate(delta * direction);
} else {
const qreal coef = qMax(fabs((camera->frame()->coordinatesOf(camera->pivotPoint())).z), 0.2 * sceneRadius);
Vec trans(0.0, 0.0, -coef * delta);
translate(inverseTransformOf(trans));
}
}
Vec Plane::getNorm(Vec x){
if(norm.x!=0||norm.y!=0||norm.z!=0) return this->norm;
Vec a = p2-p1;
Vec b = p3-p1;
Vec result = Cross(a,b);
result.norm();
this->norm = result;
return result;
};
/*! Defines the rotationConstraintDirection(). The coordinate system where \p direction is expressed depends on your class implementation. */
void AxisPlaneConstraint::setRotationConstraintDirection(const Vec& direction)
{
if ((rotationConstraintType()!=AxisPlaneConstraint::FREE) && (rotationConstraintType()!=AxisPlaneConstraint::FORBIDDEN))
{
float norm = direction.norm();
if (norm < 1E-8)
{
qWarning("AxisPlaneConstraint::setRotationConstraintDir: null vector for rotation constraint");
rotationConstraintType_ = AxisPlaneConstraint::FREE;
}
else
rotationConstraintDir_ = direction/norm;
}
}
/*! Returns the axis vector and the angle (in radians) of the rotation represented by the Quaternion.
See the axis() and angle() documentations. */
void Quaternion::getAxisAngle(Vec& axis, float& angle) const
{
angle = 2.0*acos(q[3]);
axis = Vec(q[0], q[1], q[2]);
const double sinus = axis.norm();
if (sinus > 1E-8)
axis /= sinus;
if (angle > M_PI)
{
angle = 2.0*M_PI - angle;
axis = -axis;
}
}
Ray Persp::UnProject(int U, int V, RNG &rng){
Ray result;
/*Vec centerOfPixel = leftBottomVP +
Vec(U*xDelta,V*yDelta,0) +
Vec(xDelta/2.0f,yDelta/2.0f,.0f);*/
float epsilon1 = rng.RandomFloat(); float epsilon2 = rng.RandomFloat();
Vec targetPoint = leftBottomVP + Vec(U*xDelta,V*yDelta,0) + Vec( epsilon1*xDelta, epsilon2*yDelta,.0f);
Vec dir = Vec(targetPoint.x, targetPoint.y, targetPoint.z);
dir.norm();
result = Mat4::Mul(this->viewMatInv,Ray(targetPoint, dir));
return result;
}
/*! Returns the axis vector and the angle (in radians) of the rotation represented by the Quaternion.
See the axis() and angle() documentations. */
void Quaternion::getAxisAngle(Vec& axis, qreal& angle) const
{
angle = 2.0 * acos(q[3]);
axis = Vec(q[0], q[1], q[2]);
const qreal sinus = axis.norm();
if (sinus > 1E-8)
axis /= sinus;
if (angle > M_PI)
{
angle = 2.0 * qreal(M_PI) - angle;
axis = -axis;
}
}
Number Sphere::do_intersect(const Vec & start_p, const Vec & dir) const {
Vec p = start_p - center;
Vec x = (-dir.dot(p)) * dir;
Vec y = p + x;
auto y_norm = y.norm();
if(y_norm > radius)
return -1;
auto x_dot = x.dot(dir);
auto s = std::sqrt(radius * radius - y_norm * y_norm);
auto t_norm = x_dot - s;
if(t_norm < 0)
t_norm += 2 * s;
return t_norm; // may < 0, which means no intersection
}
inline void compute_inner(int y, int w, int h, int samps, Ray &cam, Vec &cx, Vec &cy, Vec &r, Vec *c) {
//fprintf(stderr, "\rRendering (%d spp) %5.2f%%", samps * 4, 100.*y / (h - 1));
for(unsigned short x = 0, Xi[3] = { 0, 0, (unsigned short)(y*y*y) }; x < w; x++) // Loop cols
for(int sy = 0, i = (h - y - 1)*w + x; sy < 2; sy++) // 2x2 subpixel rows
for(int sx = 0; sx < 2; sx++, r = Vec()) { // 2x2 subpixel cols
for(int s = 0; s < samps; s++) {
double r1 = 2 * erand48(Xi), dx = r1 < 1 ? sqrt(r1) - 1 : 1 - sqrt(2 - r1);
double r2 = 2 * erand48(Xi), dy = r2 < 1 ? sqrt(r2) - 1 : 1 - sqrt(2 - r2);
Vec d = cx*(((sx + .5 + dx) / 2 + x) / w - .5) +
cy*(((sy + .5 + dy) / 2 + y) / h - .5) + cam.d;
r = r + radiance(Ray(cam.o + d * 140, d.norm()), 0, Xi)*(1. / samps);
} // Camera rays are pushed ^^^^^ forward to start in interior
c[i] = c[i] + Vec(clamp(r.x), clamp(r.y), clamp(r.z))*.25;
}
}
void
DGtal::Viewer3D::glDrawGLLinel(lineD3D aLinel)
{
glPushMatrix();
glTranslatef(aLinel.x1, aLinel.y1, aLinel.z1);
Vec dir (aLinel.x2-aLinel.x1, aLinel.y2-aLinel.y1, aLinel.z2-aLinel.z1 );
glMultMatrixd(Quaternion(Vec(0,0,1), dir).matrix());
GLUquadric* quadric = gluNewQuadric();
glColor4ub(aLinel.R, aLinel.G, aLinel.B, aLinel.T);
gluCylinder(quadric, (aLinel.signPos || !aLinel.isSigned) ? aLinel.width :0 ,
(aLinel.signPos && aLinel.isSigned) ? 0 :aLinel.width ,
dir.norm(),10, 4);
glPopMatrix();
}
Vec radiance(const Ray &r, int depth, unsigned short *Xi,int E=1){
double t; // distance to intersection
int id=0; // id of intersected object
if (!intersect(r, t, id)) return Vec(); // if miss, return black
const Sphere &obj = spheres[id]; // the hit object
Vec x=r.o+r.d*t, n=(x-obj.p).norm(), nl=n.dot(r.d)<0?n:n*-1, f=obj.c;
double p = f.x>f.y && f.x>f.z ? f.x : f.y>f.z ? f.y : f.z; // max refl
if (++depth>5||!p) if (erand48(Xi)<p) f=f*(1/p); else return obj.e*E;
if (obj.refl == DIFF){ // Ideal DIFFUSE reflection
double r1=2*M_PI*erand48(Xi), r2=erand48(Xi), r2s=sqrt(r2);
Vec w=nl, u=((fabs(w.x)>.1?Vec(0,1):Vec(1))%w).norm(), v=w%u;
Vec d = (u*cos(r1)*r2s + v*sin(r1)*r2s + w*sqrt(1-r2)).norm();
// Loop over any lights
Vec e;
for (int i=0; i<numSpheres; i++){
const Sphere &s = spheres[i];
if (s.e.x<=0 && s.e.y<=0 && s.e.z<=0) continue; // skip non-lights
Vec sw=s.p-x, su=((fabs(sw.x)>.1?Vec(0,1):Vec(1))%sw).norm(), sv=sw%su;
double cos_a_max = sqrt(1-s.rad*s.rad/(x-s.p).dot(x-s.p));
double eps1 = erand48(Xi), eps2 = erand48(Xi);
double cos_a = 1-eps1+eps1*cos_a_max;
double sin_a = sqrt(1-cos_a*cos_a);
double phi = 2*M_PI*eps2;
Vec l = su*cos(phi)*sin_a + sv*sin(phi)*sin_a + sw*cos_a;
l.norm();
if (intersect(Ray(x,l), t, id) && id==i){ // shadow ray
double omega = 2*M_PI*(1-cos_a_max);
e = e + f.mult(s.e*l.dot(nl)*omega)*M_1_PI; // 1/pi for brdf
}
}
return obj.e*E+e+f.mult(radiance(Ray(x,d),depth,Xi,0));
} else if (obj.refl == SPEC) // Ideal SPECULAR reflection
return obj.e + f.mult(radiance(Ray(x,r.d-n*2*n.dot(r.d)),depth,Xi));
Ray reflRay(x, r.d-n*2*n.dot(r.d)); // Ideal dielectric REFRACTION
bool into = n.dot(nl)>0; // Ray from outside going in?
double nc=1, nt=1.5, nnt=into?nc/nt:nt/nc, ddn=r.d.dot(nl), cos2t;
if ((cos2t=1-nnt*nnt*(1-ddn*ddn))<0) // Total internal reflection
return obj.e + f.mult(radiance(reflRay,depth,Xi));
Vec tdir = (r.d*nnt - n*((into?1:-1)*(ddn*nnt+sqrt(cos2t)))).norm();
double a=nt-nc, b=nt+nc, R0=a*a/(b*b), c = 1-(into?-ddn:tdir.dot(n));
double Re=R0+(1-R0)*c*c*c*c*c,Tr=1-Re,P=.25+.5*Re,RP=Re/P,TP=Tr/(1-P);
return obj.e + f.mult(depth>2 ? (erand48(Xi)<P ? // Russian roulette
radiance(reflRay,depth,Xi)*RP:radiance(Ray(x,tdir),depth,Xi)*TP) :
radiance(reflRay,depth,Xi)*Re+radiance(Ray(x,tdir),depth,Xi)*Tr);
}
void draw_segment(volume<short>& image, const Pt& p1, const Pt& p2)
{
double xdim = (double) image.xdim();
double ydim = (double) image.ydim();
double zdim = (double) image.zdim();
double mininc = min(xdim,min(ydim,zdim)) * .5;
Vec n = p1 - p2;
double d = n.norm();
n.normalize();
for (double i=0; i<=d; i+=mininc)
{
Pt p = p2 + i* n;
image((int) floor((p.X)/xdim +.5),(int) floor((p.Y)/ydim +.5),(int) floor((p.Z)/zdim +.5)) = 1;
}
}
Mat CascadeDSController::IntegrateTrajectory(realtype dt, realtype speed_threshold, realtype t_max)
{
int n_max = int(t_max/dt);
Mat traj(dim_,n_max);
Vec rpos = filt_pos_-ds_origin_;
Vec rvel = rpos;
rvel.setZero();
int n = 0;
while(n<n_max){
traj.col(n)=rpos+ds_origin_;
rvel = task_dynamics_(rpos);
if(rvel.norm()<speed_threshold){
traj.resize(dim_,n+1);
break;
}
rpos += rvel*dt;
n++;
}
return traj;
}
void draw_mesh(volume<short>& image, const Mesh &m)
{
double xdim = (double) image.xdim();
double ydim = (double) image.ydim();
double zdim = (double) image.zdim();
double mininc = min(xdim,min(ydim,zdim)) * .5;
for (list<Triangle*>::const_iterator i = m._triangles.begin(); i!=m._triangles.end(); i++)
{
Vec n = (*(*i)->get_vertice(0) - *(*i)->get_vertice(1));
double d = n.norm();
n.normalize();
for (double j=0; j<=d; j+=mininc)
{
Pt p = (*i)->get_vertice(1)->get_coord() + j* n;
draw_segment(image, p, (*i)->get_vertice(2)->get_coord());
}
}
}