double GLGPU3DDataset::Flux(int face) const
{
// TODO: pre-compute the flux
switch (face) {
case 0: return -dx() * dy() * Bz();
case 1: return -dy() * dz() * Bx();
case 2: return -dz() * dx() * By();
case 3: return dx() * dy() * Bz();
case 4: return dy() * dz() * Bx();
case 5: return dz() * dx() * By();
default: assert(false);
}
return 0.0;
}
void BaseApp::controls(){
// Compute directional vectors from euler angles
float cosX = cosf(wx), sinX = sinf(wx), cosY = cosf(wy), sinY = sinf(wy);
vec3 dx(cosY, 0, sinY);
vec3 dy(-sinX * sinY, cosX, sinX * cosY);
vec3 dz(-cosX * sinY, -sinX, cosX * cosY);
vec3 dir(0, 0, 0);
if (keys[leftKey]) dir -= dx;
if (keys[rightKey]) dir += dx;
if (keys[downKey]) dir -= dy;
if (keys[upKey]) dir += dy;
if (keys[backwardKey]) dir -= dz;
if (keys[forwardKey]) dir += dz;
float lenSq = dot(dir, dir);
if (lenSq > 0){
moveCamera(dir * (1.0f / sqrtf(lenSq)));
}
dir = vec3(0, 0, 0);
if (xStrafeAxis >= 0) dir += joystickAxes[xStrafeAxis] * (invertXStrafeAxis? -dx : dx);
if (yStrafeAxis >= 0) dir += joystickAxes[yStrafeAxis] * (invertYStrafeAxis? -dy : dy);
if (zStrafeAxis >= 0) dir += joystickAxes[zStrafeAxis] * (invertZStrafeAxis? -dz : dz);
if (dot(dir, dir) > 0){
moveCamera(dir);
}
if (xTurnAxis >= 0) wx += (invertXTurnAxis? -2.0f : 2.0f) * joystickAxes[xTurnAxis] * frameTime;
if (yTurnAxis >= 0) wy += (invertYTurnAxis? -2.0f : 2.0f) * joystickAxes[yTurnAxis] * frameTime;
}
void ShapeDrawer::drawShape(const btCollisionShape* shape)
{
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
btVector3 org(0,0,0);
btVector3 dx(1,0,0);
btVector3 dy(0,1,0);
btVector3 dz(0,0,1);
dx *= halfExtent[0];
dy *= halfExtent[1];
dz *= halfExtent[2];
glBegin(GL_LINE_LOOP);
glDrawVector(org - dx - dy - dz);
glDrawVector(org + dx - dy - dz);
glDrawVector(org + dx + dy - dz);
glDrawVector(org - dx + dy - dz);
glDrawVector(org - dx + dy + dz);
glDrawVector(org + dx + dy + dz);
glDrawVector(org + dx - dy + dz);
glDrawVector(org - dx - dy + dz);
glEnd();
glBegin(GL_LINES);
glDrawVector(org + dx - dy - dz);
glDrawVector(org + dx - dy + dz);
glDrawVector(org + dx + dy - dz);
glDrawVector(org + dx + dy + dz);
glDrawVector(org - dx - dy - dz);
glDrawVector(org - dx + dy - dz);
glDrawVector(org - dx - dy + dz);
glDrawVector(org - dx + dy + dz);
glEnd();
}
Type dist(const Vec3& src) const
{
Type dx(x - src.x);
Type dy(y - src.y);
Type dz(z - src.z);
return _sqrt(dx * dx + dy * dy + dz *dz);
}
double Segment3D::length() {
double d_x = dx();
double d_y = dy();
double d_z = dz();
return sqrt( d_x*d_x + d_y*d_y + d_z*d_z );
};
bool Sphere::IntersectP(const Ray &r, bool testAlphaTexture) const {
Float phi;
Point3f pHit;
// Transform _Ray_ to object space
Vector3f oErr, dErr;
Ray ray = (*WorldToObject)(r, &oErr, &dErr);
// Compute quadratic sphere coefficients
// Initialize _EFloat_ ray coordinate values
EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
EFloat a = dx * dx + dy * dy + dz * dz;
EFloat b = 2 * (dx * ox + dy * oy + dz * oz);
EFloat c = ox * ox + oy * oy + oz * oz - EFloat(radius) * EFloat(radius);
// Solve quadratic equation for _t_ values
EFloat t0, t1;
if (!Quadratic(a, b, c, &t0, &t1)) return false;
// Check quadric shape _t0_ and _t1_ for nearest intersection
if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
EFloat tShapeHit = t0;
if (tShapeHit.LowerBound() <= 0) {
tShapeHit = t1;
if (tShapeHit.UpperBound() > ray.tMax) return false;
}
// Compute sphere hit position and $\phi$
pHit = ray((Float)tShapeHit);
// Refine sphere intersection point
pHit *= radius / Distance(pHit, Point3f(0, 0, 0));
if (pHit.x == 0 && pHit.y == 0) pHit.x = 1e-5f * radius;
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0) phi += 2 * Pi;
// Test sphere intersection against clipping parameters
if ((zMin > -radius && pHit.z < zMin) || (zMax < radius && pHit.z > zMax) ||
phi > phiMax) {
if (tShapeHit == t1) return false;
if (t1.UpperBound() > ray.tMax) return false;
tShapeHit = t1;
// Compute sphere hit position and $\phi$
pHit = ray((Float)tShapeHit);
// Refine sphere intersection point
pHit *= radius / Distance(pHit, Point3f(0, 0, 0));
if (pHit.x == 0 && pHit.y == 0) pHit.x = 1e-5f * radius;
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0) phi += 2 * Pi;
if ((zMin > -radius && pHit.z < zMin) ||
(zMax < radius && pHit.z > zMax) || phi > phiMax)
return false;
}
return true;
}
//------------------------------------------------------------------------------
//!
String
Event::toStr() const
{
String tmp;
tmp += "{";
tmp += typeToStr(type());
tmp += ",";
tmp += String().format("t=%f", timestamp());
tmp += ",";
switch( type() )
{
case POINTER_PRESS :
case POINTER_RELEASE:
case POINTER_MOVE :
case POINTER_ENTER :
case POINTER_LEAVE :
case POINTER_CHANGE :
case POINTER_SCROLL :
case POINTER_DELETE :
case POINTER_CANCEL :
{
tmp += String().format( "ptrID=%d", pointerID() );
tmp += ",";
tmp += String().format( "val=%d", value() );
tmp += ",";
tmp += String().format( "pos=(%f,%f)", position().x, position().y );
tmp += ",";
tmp += String().format( "cnt=%d", count() );
} break;
case KEY_PRESS:
case KEY_RELEASE:
case CHAR:
{
tmp += String().format( "val=%d", value() );
tmp += ",";
tmp += String().format( "cnt=%d", count() );
} break;
case HID_EVENT:
{
tmp += String().format( "devTypeID=%d", deviceTypeID() );
tmp += ",";
tmp += String().format( "devID=%d", deviceID() );
tmp += ",";
tmp += String().format( "ctrlID=%d", controlID() );
tmp += ",";
tmp += String().format( "val=%f", valueFloat() );
} break;
case ACCELERATE:
{
tmp += String().format( "(%f,%f,%f", dx(), dy(), dz() );
} break;
default:
break;
}
tmp += "}";
return tmp;
}
void GLGPU3DDataset::ComputeSupercurrentField(int slot)
{
const int nvoxels = dims()[0]*dims()[1]*dims()[2];
if (_J[slot] != NULL) free(_J[slot]);
_J[slot] = (double*)malloc(3*sizeof(double)*nvoxels);
double *J = _J[slot];
memset(J, 0, 3*sizeof(double)*nvoxels);
double u, v, rho2;
double du[3], dv[3], A[3], j[3];
// central difference
for (int x=1; x<dims()[0]-1; x++) {
for (int y=1; y<dims()[1]-1; y++) {
for (int z=1; z<dims()[2]-1; z++) {
int idx[3] = {x, y, z};
double pos[3];
Idx2Pos(idx, pos);
du[0] = 0.5 * (Re(x+1, y, z, slot) - Re(x-1, y, z, slot)) / dx();
du[1] = 0.5 * (Re(x, y+1, z, slot) - Re(x, y-1, z, slot)) / dy();
du[2] = 0.5 * (Re(x, y, z+1, slot) - Re(x, y, z-1, slot)) / dz();
dv[0] = 0.5 * (Im(x+1, y, z, slot) - Im(x-1, y, z, slot)) / dx();
dv[1] = 0.5 * (Im(x, y+1, z, slot) - Im(x, y-1, z, slot)) / dy();
dv[2] = 0.5 * (Im(x, y, z+1, slot) - Im(x, y, z-1, slot)) / dz();
this->A(pos, A, slot);
u = Re(x, y, z, slot);
v = Im(x, y, z, slot);
rho2 = u*u + v*v;
texel3Dv(J, dims(), 3, x, y, z, 0) = j[0] = (u*dv[0] - v*du[0]) - rho2*(A[0] + Kex()); // + Kex();
texel3Dv(J, dims(), 3, x, y, z, 1) = j[1] = (u*dv[1] - v*du[1]) - rho2*A[1];
texel3Dv(J, dims(), 3, x, y, z, 2) = j[2] = (u*dv[2] - v*du[2]) - rho2*A[2];
// fprintf(stderr, "J={%f, %f, %f}\n", j[0], j[1], j[2]);
}
}
}
}
bool Cylinder::IntersectP(const Ray &r, bool testAlphaTexture) const {
Float phi;
Point3f pHit;
// Transform _Ray_ to object space
Vector3f oErr, dErr;
Ray ray = (*WorldToObject)(r, &oErr, &dErr);
// Compute quadratic cylinder coefficients
// Initialize _EFloat_ ray coordinate values
EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
EFloat a = dx * dx + dy * dy;
EFloat b = 2 * (dx * ox + dy * oy);
EFloat c = ox * ox + oy * oy - EFloat(radius) * EFloat(radius);
// Solve quadratic equation for _t_ values
EFloat t0, t1;
if (!Quadratic(a, b, c, &t0, &t1)) return false;
// Check quadric shape _t0_ and _t1_ for nearest intersection
if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
EFloat tShapeHit = t0;
if (tShapeHit.LowerBound() <= 0) {
tShapeHit = t1;
if (tShapeHit.UpperBound() > ray.tMax) return false;
}
// Compute cylinder hit point and $\phi$
pHit = ray((Float)tShapeHit);
// Refine cylinder intersection point
Float hitRad = std::sqrt(pHit.x * pHit.x + pHit.y * pHit.y);
pHit.x *= radius / hitRad;
pHit.y *= radius / hitRad;
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0) phi += 2 * Pi;
// Test cylinder intersection against clipping parameters
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) {
if (tShapeHit == t1) return false;
tShapeHit = t1;
if (t1.UpperBound() > ray.tMax) return false;
// Compute cylinder hit point and $\phi$
pHit = ray((Float)tShapeHit);
// Refine cylinder intersection point
Float hitRad = std::sqrt(pHit.x * pHit.x + pHit.y * pHit.y);
pHit.x *= radius / hitRad;
pHit.y *= radius / hitRad;
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0) phi += 2 * Pi;
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false;
}
return true;
}
/*!
Compute the point of the line with a given Z component
\param theZLevel
Z component to use in point computation
\return
Point on the line with Z component equal to theZLevel, if theZLevel is less then minim Z of the line return
a point with -DBL_MAX in its components, if it is greater then maximum Z return a point with DBL_MAX in its
components
If the line is not valid return an invalid point
If the line is horizontal with Z component equal to theZLevel return the begin point of the line
*/
GM_3dPoint GM_3dLine::pointAtZ(double theZLevel) const {
if (!isValid()) {
return GM_3dPoint();
}
if (theZLevel < minZ() - GM_DIFF_TOLERANCE) {
return GM_3dPoint(-DBL_MAX, -DBL_MAX, -DBL_MAX);
}
if (theZLevel > maxZ() + GM_DIFF_TOLERANCE) {
return GM_3dPoint(DBL_MAX, DBL_MAX, DBL_MAX);
}
if (dz() < GM_NULL_TOLERANCE && fabs(theZLevel - mBegin.z()) < GM_DIFF_TOLERANCE) {
return mBegin;
}
else {
double ratio = fabs(theZLevel - begin().z()) / fabs(dz());
return GM_3dPoint(begin().x() + dx()*ratio, begin().y() + dy()*ratio, theZLevel);
}
}
void CSceneNodeAABB::Render() const{
if(!GetSingleton<CProgram>()->IsInWireFrameMode())
{
//glPushAttrib(GL_ALL_ATTRIB_BITS);
////glBegin();
//glEnable(GL_MAP2_VERTEX_3);
//vector3 r[4] = {
// m_aabb.first ,
// vector3(m_aabb.first.x,m_aabb.second.y,m_aabb.first.z) ,
// vector3(m_aabb.second.x,m_aabb.first.y,m_aabb.first.z) ,
// vector3(m_aabb.second.x,m_aabb.second.y,m_aabb.first.z)
//};
//glMap2f( GL_MAP2_VERTEX_3,0.0f,1.0f,6,2,0.0f,1.0f,3,2,(GLfloat*)r);
//glMapGrid2f(10,0,1,20,0,1);
//glEvalMesh2(GL_FILL,0,10,0,20);
////glEnd();
//glPopAttrib();
vector3 dim(GetDim(m_aabb));
vector3 dx(dim.x,0,0);
vector3 dy(0,dim.y,0);
vector3 dz(0,0,dim.z);
unsigned int nx(dim.x*0.02+1);
unsigned int ny(dim.y*0.02+1);
unsigned int nz(dim.z*0.02+1);
DrawFace(m_aabb.first,dy,dx,ny,nx);
DrawFace(m_aabb.first,dx,dz,nx,nz);
DrawFace(m_aabb.first,dz,dy,nz,ny);
DrawFace(m_aabb.second,-dx,-dy,nx,ny);
DrawFace(m_aabb.second,-dy,-dz,ny,nz);
DrawFace(m_aabb.second,-dz,-dx,nz,nx);
return;
}
glPushAttrib(GL_TEXTURE_BIT);
glPushMatrix();
vector3 center(GetCenter(m_aabb)),dim(GetDim(m_aabb));
glTranslatef(center.x,center.y,center.z);
{
GLenum coord[2]={GL_S,GL_T};
for(int iCoord(0); iCoord<2; ++iCoord)
{
float f[4];
glGetTexGenfv(coord[iCoord],GL_OBJECT_PLANE,f);
f[0]*=dim.x*0.001f;
f[1]*=dim.y*0.001f;
f[2]*=dim.z*0.001f;
//f[2]*=1000;
glTexGenfv(coord[iCoord],GL_OBJECT_PLANE,f);
}
}
glScalef(dim.x,dim.y,dim.z);
glutSolidCube(1.0);
glPopMatrix();
glPopAttrib();
}
bool Hyperboloid::IntersectP(const Ray &r) const {
Float phi, v;
Point3f pHit;
// Transform _Ray_ to object space
Vector3f oErr, dErr;
Ray ray = (*WorldToObject)(r, &oErr, &dErr);
// Compute quadratic hyperboloid coefficients
// Initialize _EFloat_ ray coordinate values
EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
EFloat a = ah * dx * dx + ah * dy * dy - ch * dz * dz;
EFloat b = 2.f * (ah * dx * ox + ah * dy * oy - ch * dz * oz);
EFloat c = ah * ox * ox + ah * oy * oy - ch * oz * oz - 1.f;
// Solve quadratic equation for _t_ values
EFloat t0, t1;
if (!Quadratic(a, b, c, &t0, &t1)) return false;
// Check quadric shape _t0_ and _t1_ for nearest intersection
if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
EFloat tShapeHit = t0;
if (t0.LowerBound() <= 0) {
tShapeHit = t1;
if (tShapeHit.UpperBound() > ray.tMax) return false;
}
// Compute hyperboloid inverse mapping
pHit = ray((Float)tShapeHit);
v = (pHit.z - p1.z) / (p2.z - p1.z);
Point3f pr = (1 - v) * p1 + v * p2;
phi = std::atan2(pr.x * pHit.y - pHit.x * pr.y,
pHit.x * pr.x + pHit.y * pr.y);
if (phi < 0) phi += 2 * Pi;
// Test hyperboloid intersection against clipping parameters
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) {
if (tShapeHit == t1) return false;
tShapeHit = t1;
if (t1.UpperBound() > ray.tMax) return false;
// Compute hyperboloid inverse mapping
pHit = ray((Float)tShapeHit);
v = (pHit.z - p1.z) / (p2.z - p1.z);
Point3f pr = (1 - v) * p1 + v * p2;
phi = std::atan2(pr.x * pHit.y - pHit.x * pr.y,
pHit.x * pr.x + pHit.y * pr.y);
if (phi < 0) phi += 2 * Pi;
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false;
}
return true;
}
/**
Compute the displacements of the structure (in global coordinate system)
Return:
a tuple containing (x, y, z, theta_x, theta_y, theta_z).
each entry of the tuple is a numpy array describing the deflections for the given
degree of freedom at each node.
**/
py::tuple computeDisplacement(){
int nodes = beam->getNumNodes();
Vector dx(nodes);
Vector dy(nodes);
Vector dz(nodes);
Vector dtx(nodes);
Vector dty(nodes);
Vector dtz(nodes);
beam->computeDisplacement(dx, dy, dz, dtx, dty, dtz);
return py::make_tuple(dx, dy, dz, dtx, dty, dtz);
}
bool Paraboloid::IntersectP(const Ray &r) const {
Float phi;
Point3f pHit;
// Transform _Ray_ to object space
Vector3f oErr, dErr;
Ray ray = (*WorldToObject)(r, &oErr, &dErr);
// Compute quadratic paraboloid coefficients
// Initialize _EFloat_ ray coordinate values
EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
EFloat k = EFloat(zMax) / (EFloat(radius) * EFloat(radius));
EFloat a = k * (dx * dx + dy * dy);
EFloat b = 2.f * k * (dx * ox + dy * oy) - dz;
EFloat c = k * (ox * ox + oy * oy) - oz;
// Solve quadratic equation for _t_ values
EFloat t0, t1;
if (!Quadratic(a, b, c, &t0, &t1)) return false;
// Check quadric shape _t0_ and _t1_ for nearest intersection
if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
EFloat tShapeHit = t0;
if (tShapeHit.LowerBound() <= 0) {
tShapeHit = t1;
if (tShapeHit.UpperBound() > ray.tMax) return false;
}
// Compute paraboloid inverse mapping
pHit = ray((Float)tShapeHit);
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0.) phi += 2 * Pi;
// Test paraboloid intersection against clipping parameters
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) {
if (tShapeHit == t1) return false;
tShapeHit = t1;
if (t1.UpperBound() > ray.tMax) return false;
// Compute paraboloid inverse mapping
pHit = ray((Float)tShapeHit);
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0.) phi += 2 * Pi;
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false;
}
return true;
}
Vector3 MeshEvolution::scalarFieldNormal(const Vector3 &pos) const
{
const float e = 1e-2;
Vector3 dx(e, 0, 0);
Vector3 dy(0, e, 0);
Vector3 dz(0, 0, e);
Vector3 normal(
scalarField(pos + dx) - scalarField(pos - dx),
scalarField(pos + dy) - scalarField(pos - dy),
scalarField(pos + dz) - scalarField(pos - dz)
);
float len = normal.length();
if (len < 1e-6) {
return Vector3();
}
return normal / len;
}
/*!
Compute a point on the line from a parameter in [0,1] interval, where 0 is the begin and 1 is the end
of the line
\param theSection
Line section to use for point computation
\return
Point on the line at the section theSectio, return an invalid point if the line is invalid or theSection is
out of range
*/
GM_3dPoint GM_3dLine::pointFromSection(double theSection) const {
GM_3dPoint ret;
if (!isValid() || theSection < -GM_DIFF_TOLERANCE || theSection > 1.0 + GM_DIFF_TOLERANCE) {
return ret;
}
if (theSection < 0.0) {
theSection = 0.0;
}
if (theSection > 1.0) {
theSection = 1.0;
}
theSection *= length();
GM_3dVector lineDir(dx(), dy(), dz());
lineDir.normalize();
ret = (GM_3dVector)mBegin + (lineDir * theSection);
return ret;
}
/*!
From a point on the line compute the section on the line corrisponding to the specified point, the section
is in [0,1] interval, where 0 is the begin and 1 is the end of the line.
\param thePoint
Point on the line to use for section computation
\return
Section of the line computed from thePoint, 0 = begin 1 = end, return -DBL_MAX if the line or thePoinr are
not valid or if thePoint is not on the line
*/
double GM_3dLine::sectionFromPoint(GM_3dPoint thePoint) const {
double ret = -DBL_MAX;
if (!isValid() || !thePoint.isValid())
return ret;
GM_3dVector lineDir(dx(), dy(), dz());
lineDir.normalize();
GM_3dVector pointDir(thePoint.x() - mBegin.x(), thePoint.y() - mBegin.y(), thePoint.z() - mBegin.z());
pointDir.normalize();
if (fabs((lineDir * pointDir) - 1.0) < GM_DIFF_TOLERANCE) {
double distFromBegin = thePoint.distFrom(mBegin);
double distFromEnd = thePoint.distFrom(mEnd);
double lineLen = length();
if (fabs(distFromBegin + distFromEnd - lineLen) < GM_DIFF_TOLERANCE) {
ret = distFromBegin / lineLen;
}
}
return ret;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// DEFINE VALRIABLES:
// ---------------------------
DifferentialState x, y;
Function f;
f << x*x + pow(y,2);
// TEST THE FUNCTION f:
// --------------------
EvaluationPoint z(f);
EvaluationPoint dz(f);
Vector xx(2); Vector dx(2);
xx(0) = 1.0; dx(0) = 0.5;
xx(1) = 1.0; dx(1) = 0.1;
z.setX( xx ); dz.setX( dx );
// FORWARD DIFFERENTIATION:
// ------------------------
Vector ff = f.evaluate ( z );
Vector df = f.AD_forward( dz );
// PRINT THE RESULTS:
// ------------------
ff.print("result of evaluation \n");
df.print("result for the derivative \n");
return 0;
}
bool Hyperboloid::Intersect(const Ray &r, Float *tHit,
SurfaceInteraction *isect) const {
Float phi, v;
Point3f pHit;
// Transform _Ray_ to object space
Vector3f oErr, dErr;
Ray ray = (*WorldToObject)(r, &oErr, &dErr);
// Compute quadratic hyperboloid coefficients
// Initialize _EFloat_ ray coordinate values
EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
EFloat a = ah * dx * dx + ah * dy * dy - ch * dz * dz;
EFloat b = 2.f * (ah * dx * ox + ah * dy * oy - ch * dz * oz);
EFloat c = ah * ox * ox + ah * oy * oy - ch * oz * oz - 1.f;
// Solve quadratic equation for _t_ values
EFloat t0, t1;
if (!Quadratic(a, b, c, &t0, &t1)) return false;
// Check quadric shape _t0_ and _t1_ for nearest intersection
if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
EFloat tShapeHit = t0;
if (t0.LowerBound() <= 0) {
tShapeHit = t1;
if (tShapeHit.UpperBound() > ray.tMax) return false;
}
// Compute hyperboloid inverse mapping
pHit = ray((Float)tShapeHit);
v = (pHit.z - p1.z) / (p2.z - p1.z);
Point3f pr = (1 - v) * p1 + v * p2;
phi = std::atan2(pr.x * pHit.y - pHit.x * pr.y,
pHit.x * pr.x + pHit.y * pr.y);
if (phi < 0) phi += 2 * Pi;
// Test hyperboloid intersection against clipping parameters
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) {
if (tShapeHit == t1) return false;
tShapeHit = t1;
if (t1.UpperBound() > ray.tMax) return false;
// Compute hyperboloid inverse mapping
pHit = ray((Float)tShapeHit);
v = (pHit.z - p1.z) / (p2.z - p1.z);
Point3f pr = (1 - v) * p1 + v * p2;
phi = std::atan2(pr.x * pHit.y - pHit.x * pr.y,
pHit.x * pr.x + pHit.y * pr.y);
if (phi < 0) phi += 2 * Pi;
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false;
}
// Compute parametric representation of hyperboloid hit
Float u = phi / phiMax;
// Compute hyperboloid $\dpdu$ and $\dpdv$
Float cosPhi = std::cos(phi), sinPhi = std::sin(phi);
Vector3f dpdu(-phiMax * pHit.y, phiMax * pHit.x, 0.);
Vector3f dpdv((p2.x - p1.x) * cosPhi - (p2.y - p1.y) * sinPhi,
(p2.x - p1.x) * sinPhi + (p2.y - p1.y) * cosPhi, p2.z - p1.z);
// Compute hyperboloid $\dndu$ and $\dndv$
Vector3f d2Pduu = -phiMax * phiMax * Vector3f(pHit.x, pHit.y, 0);
Vector3f d2Pduv = phiMax * Vector3f(-dpdv.y, dpdv.x, 0.);
Vector3f d2Pdvv(0, 0, 0);
// Compute coefficients for fundamental forms
Float E = Dot(dpdu, dpdu);
Float F = Dot(dpdu, dpdv);
Float G = Dot(dpdv, dpdv);
Vector3f N = Normalize(Cross(dpdu, dpdv));
Float e = Dot(N, d2Pduu);
Float f = Dot(N, d2Pduv);
Float g = Dot(N, d2Pdvv);
// Compute $\dndu$ and $\dndv$ from fundamental form coefficients
Float invEGF2 = 1 / (E * G - F * F);
Normal3f dndu = Normal3f((f * F - e * G) * invEGF2 * dpdu +
(e * F - f * E) * invEGF2 * dpdv);
Normal3f dndv = Normal3f((g * F - f * G) * invEGF2 * dpdu +
(f * F - g * E) * invEGF2 * dpdv);
// Compute error bounds for hyperboloid intersection
// Compute error bounds for intersection computed with ray equation
EFloat px = ox + tShapeHit * dx;
EFloat py = oy + tShapeHit * dy;
EFloat pz = oz + tShapeHit * dz;
Vector3f pError = Vector3f(px.GetAbsoluteError(), py.GetAbsoluteError(),
pz.GetAbsoluteError());
// Initialize _SurfaceInteraction_ from parametric information
*isect = (*ObjectToWorld)(SurfaceInteraction(pHit, pError, Point2f(u, v),
-ray.d, dpdu, dpdv, dndu, dndv,
ray.time, this));
*tHit = (Float)tShapeHit;
return true;
}
osg::Node* createCube(unsigned int mask)
{
osg::Geode* geode = new osg::Geode;
osg::Geometry* geometry = new osg::Geometry;
geode->addDrawable(geometry);
osg::Vec3Array* vertices = new osg::Vec3Array;
geometry->setVertexArray(vertices);
osg::Vec3Array* normals = new osg::Vec3Array;
geometry->setNormalArray(normals, osg::Array::BIND_PER_VERTEX);
osg::Vec4Array* colours = new osg::Vec4Array;
geometry->setColorArray(colours, osg::Array::BIND_OVERALL);
colours->push_back(osg::Vec4(1.0f,1.0f,1.0f,1.0f));
osg::Vec3 origin(0.0f,0.0f,0.0f);
osg::Vec3 dx(2.0f,0.0f,0.0f);
osg::Vec3 dy(0.0f,1.0f,0.0f);
osg::Vec3 dz(0.0f,0.0f,1.0f);
osg::Vec3 px(1.0f,0.0,0.0f);
osg::Vec3 nx(-1.0f,0.0,0.0f);
osg::Vec3 py(0.0f,1.0f,0.0f);
osg::Vec3 ny(0.0f,-1.0f,0.0f);
osg::Vec3 pz(0.0f,0.0f,1.0f);
osg::Vec3 nz(0.0f,0.0f,-1.0f);
if (mask & FRONT_FACE)
{
// front face
vertices->push_back(origin);
vertices->push_back(origin+dx);
vertices->push_back(origin+dx+dz);
vertices->push_back(origin+dz);
normals->push_back(ny);
normals->push_back(ny);
normals->push_back(ny);
normals->push_back(ny);
}
if (mask & BACK_FACE)
{
// back face
vertices->push_back(origin+dy);
vertices->push_back(origin+dy+dz);
vertices->push_back(origin+dy+dx+dz);
vertices->push_back(origin+dy+dx);
normals->push_back(py);
normals->push_back(py);
normals->push_back(py);
normals->push_back(py);
}
if (mask & LEFT_FACE)
{
// left face
vertices->push_back(origin+dy);
vertices->push_back(origin);
vertices->push_back(origin+dz);
vertices->push_back(origin+dy+dz);
normals->push_back(nx);
normals->push_back(nx);
normals->push_back(nx);
normals->push_back(nx);
}
if (mask & RIGHT_FACE)
{
// right face
vertices->push_back(origin+dx+dy);
vertices->push_back(origin+dx+dy+dz);
vertices->push_back(origin+dx+dz);
vertices->push_back(origin+dx);
normals->push_back(px);
normals->push_back(px);
normals->push_back(px);
normals->push_back(px);
}
if (mask & TOP_FACE)
{
// top face
vertices->push_back(origin+dz);
vertices->push_back(origin+dz+dx);
vertices->push_back(origin+dz+dx+dy);
vertices->push_back(origin+dz+dy);
normals->push_back(pz);
normals->push_back(pz);
normals->push_back(pz);
normals->push_back(pz);
}
if (mask & BOTTOM_FACE)
{
// bottom face
vertices->push_back(origin);
vertices->push_back(origin+dy);
vertices->push_back(origin+dx+dy);
vertices->push_back(origin+dx);
normals->push_back(nz);
normals->push_back(nz);
normals->push_back(nz);
normals->push_back(nz);
}
geometry->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 0, vertices->size()));
return geode;
}
void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax)
{
if (shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE)
{
btVector3 org(m[12], m[13], m[14]);
btVector3 dx(m[0], m[1], m[2]);
btVector3 dy(m[4], m[5], m[6]);
// btVector3 dz(m[8], m[9], m[10]);
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
dx *= halfExtent[0];
dy *= halfExtent[1];
// dz *= halfExtent[2];
glColor3f(1,1,1);
glDisable(GL_LIGHTING);
glLineWidth(2);
glBegin(GL_LINE_LOOP);
glDrawVector(org - dx - dy);
glDrawVector(org - dx + dy);
glDrawVector(org + dx + dy);
glDrawVector(org + dx - dy);
glEnd();
return;
}
else if((shape->getShapeType() == BOX_SHAPE_PROXYTYPE) && (debugMode & btIDebugDraw::DBG_FastWireframe))
{
btVector3 org(m[12], m[13], m[14]);
btVector3 dx(m[0], m[1], m[2]);
btVector3 dy(m[4], m[5], m[6]);
btVector3 dz(m[8], m[9], m[10]);
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
dx *= halfExtent[0];
dy *= halfExtent[1];
dz *= halfExtent[2];
glBegin(GL_LINE_LOOP);
glDrawVector(org - dx - dy - dz);
glDrawVector(org + dx - dy - dz);
glDrawVector(org + dx + dy - dz);
glDrawVector(org - dx + dy - dz);
glDrawVector(org - dx + dy + dz);
glDrawVector(org + dx + dy + dz);
glDrawVector(org + dx - dy + dz);
glDrawVector(org - dx - dy + dz);
glEnd();
glBegin(GL_LINES);
glDrawVector(org + dx - dy - dz);
glDrawVector(org + dx - dy + dz);
glDrawVector(org + dx + dy - dz);
glDrawVector(org + dx + dy + dz);
glDrawVector(org - dx - dy - dz);
glDrawVector(org - dx + dy - dz);
glDrawVector(org - dx - dy + dz);
glDrawVector(org - dx + dy + dz);
glEnd();
return;
}
glPushMatrix();
btglMultMatrix(m);
if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE)
{
const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape);
const btConvexShape* convexShape = scalingShape->getChildShape();
float scalingFactor = (float)scalingShape->getUniformScalingFactor();
{
btScalar tmpScaling[4][4]={{scalingFactor,0,0,0},
{0,scalingFactor,0,0},
{0,0,scalingFactor,0},
{0,0,0,1}};
drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode,worldBoundsMin,worldBoundsMax);
}
glPopMatrix();
return;
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
ATTRIBUTE_ALIGNED16(btScalar) childMat[16];
childTrans.getOpenGLMatrix(childMat);
drawOpenGL(childMat,colShape,color,debugMode,worldBoundsMin,worldBoundsMax);
}
} else
{
if(m_textureenabled&&(!m_textureinitialized))
{
GLubyte* image=new GLubyte[256*256*4];
for(int y=0;y<256;++y)
{
const int t=y>>4;
GLubyte* pi=image+y*256*3;
for(int x=0;x<256;++x)
{
const int s=x>>4;
const GLubyte b=180;
GLubyte c=b+((s+(t&1))&1)*(255-b);
pi[0]=pi[1]=pi[2]=pi[3]=c;pi+=3;
}
}
glGenTextures(1,(GLuint*)&m_texturehandle);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
glGenTextures(1,(GLuint*)&m_texturehandle);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
glTexImage2D(GL_TEXTURE_2D, 0, 3, 256 , 256 , 0, GL_RGB, GL_UNSIGNED_BYTE, image);
//glGenerateMipmap(GL_TEXTURE_2D);
delete[] image;
}
glMatrixMode(GL_TEXTURE);
glLoadIdentity();
glScalef(0.025f,0.025f,0.025f);
glMatrixMode(GL_MODELVIEW);
static const GLfloat planex[]={1,0,0,0};
// static const GLfloat planey[]={0,1,0,0};
static const GLfloat planez[]={0,0,1,0};
glTexGenfv(GL_S,GL_OBJECT_PLANE,planex);
glTexGenfv(GL_T,GL_OBJECT_PLANE,planez);
glTexGeni(GL_S,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
glTexGeni(GL_T,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
glEnable(GL_TEXTURE_GEN_S);
glEnable(GL_TEXTURE_GEN_T);
glEnable(GL_TEXTURE_GEN_R);
m_textureinitialized=true;
//drawCoordSystem();
//glPushMatrix();
glEnable(GL_COLOR_MATERIAL);
if(m_textureenabled)
{
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
} else
{
glDisable(GL_TEXTURE_2D);
}
glColor3f(color.x(),color.y(), color.z());
//bool useWireframeFallback = true;
if (!(debugMode & btIDebugDraw::DBG_DrawWireframe))
{
///you can comment out any of the specific cases, and use the default
///the benefit of 'default' is that it approximates the actual collision shape including collision margin
//int shapetype=m_textureenabled?MAX_BROADPHASE_COLLISION_TYPES:shape->getShapeType();
int shapetype=shape->getShapeType();
switch (shapetype)
{
case SPHERE_SHAPE_PROXYTYPE:
{
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
drawSphere(radius,10,10);
//useWireframeFallback = false;
break;
}
case BOX_SHAPE_PROXYTYPE:
{
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
static int indices[36] = {
0,1,2,
3,2,1,
4,0,6,
6,0,2,
5,1,4,
4,1,0,
7,3,1,
7,1,5,
5,4,7,
7,4,6,
7,2,3,
7,6,2};
btVector3 vertices[8]={
btVector3(halfExtent[0],halfExtent[1],halfExtent[2]),
btVector3(-halfExtent[0],halfExtent[1],halfExtent[2]),
btVector3(halfExtent[0],-halfExtent[1],halfExtent[2]),
btVector3(-halfExtent[0],-halfExtent[1],halfExtent[2]),
btVector3(halfExtent[0],halfExtent[1],-halfExtent[2]),
btVector3(-halfExtent[0],halfExtent[1],-halfExtent[2]),
btVector3(halfExtent[0],-halfExtent[1],-halfExtent[2]),
btVector3(-halfExtent[0],-halfExtent[1],-halfExtent[2])};
#if 1
glBegin (GL_TRIANGLES);
int si=36;
for (int i=0;i<si;i+=3)
{
const btVector3& v1 = vertices[indices[i]];;
const btVector3& v2 = vertices[indices[i+1]];
const btVector3& v3 = vertices[indices[i+2]];
btVector3 normal = (v3-v1).cross(v2-v1);
normal.normalize ();
glNormal3f(normal.getX(),normal.getY(),normal.getZ());
glVertex3f (v1.x(), v1.y(), v1.z());
glVertex3f (v2.x(), v2.y(), v2.z());
glVertex3f (v3.x(), v3.y(), v3.z());
}
glEnd();
#endif
//useWireframeFallback = false;
break;
}
#if 0
case CONE_SHAPE_PROXYTYPE:
{
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
int upIndex = coneShape->getConeUpIndex();
float radius = coneShape->getRadius();//+coneShape->getMargin();
float height = coneShape->getHeight();//+coneShape->getMargin();
switch (upIndex)
{
case 0:
glRotatef(90.0, 0.0, 1.0, 0.0);
break;
case 1:
glRotatef(-90.0, 1.0, 0.0, 0.0);
break;
case 2:
break;
default:
{
}
};
glTranslatef(0.0, 0.0, -0.5*height);
glutSolidCone(radius,height,10,10);
//useWireframeFallback = false;
break;
}
#endif
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btScalar vecLen = 100.f;
btVector3 pt0 = planeOrigin + vec0*vecLen;
btVector3 pt1 = planeOrigin - vec0*vecLen;
btVector3 pt2 = planeOrigin + vec1*vecLen;
btVector3 pt3 = planeOrigin - vec1*vecLen;
glBegin(GL_LINES);
glVertex3f(pt0.getX(),pt0.getY(),pt0.getZ());
glVertex3f(pt1.getX(),pt1.getY(),pt1.getZ());
glVertex3f(pt2.getX(),pt2.getY(),pt2.getZ());
glVertex3f(pt3.getX(),pt3.getY(),pt3.getZ());
glEnd();
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
btTransform childTransform;
childTransform.setIdentity();
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
btSphereShape sc(multiSphereShape->getSphereRadius(i));
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
ATTRIBUTE_ALIGNED16(btScalar) childMat[16];
childTransform.getOpenGLMatrix(childMat);
drawOpenGL(childMat,&sc,color,debugMode,worldBoundsMin,worldBoundsMax);
}
break;
}
default:
{
if (shape->isConvex())
{
const btConvexPolyhedron* poly = shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron() : 0;
if (poly)
{
int i;
glBegin (GL_TRIANGLES);
for (i=0;i<poly->m_faces.size();i++)
{
btVector3 centroid(0,0,0);
int numVerts = poly->m_faces[i].m_indices.size();
if (numVerts>2)
{
btVector3 v1 = poly->m_vertices[poly->m_faces[i].m_indices[0]];
for (int v=0;v<poly->m_faces[i].m_indices.size()-2;v++)
{
btVector3 v2 = poly->m_vertices[poly->m_faces[i].m_indices[v+1]];
btVector3 v3 = poly->m_vertices[poly->m_faces[i].m_indices[v+2]];
btVector3 normal = (v3-v1).cross(v2-v1);
normal.normalize ();
glNormal3f(normal.getX(),normal.getY(),normal.getZ());
glVertex3f (v1.x(), v1.y(), v1.z());
glVertex3f (v2.x(), v2.y(), v2.z());
glVertex3f (v3.x(), v3.y(), v3.z());
}
}
}
glEnd ();
} else
{
ShapeCache* sc=cache((btConvexShape*)shape);
//glutSolidCube(1.0);
btShapeHull* hull = &sc->m_shapehull/*(btShapeHull*)shape->getUserPointer()*/;
if (hull->numTriangles () > 0)
{
int index = 0;
const unsigned int* idx = hull->getIndexPointer();
const btVector3* vtx = hull->getVertexPointer();
glBegin (GL_TRIANGLES);
for (int i = 0; i < hull->numTriangles (); i++)
{
int i1 = index++;
int i2 = index++;
int i3 = index++;
btAssert(i1 < hull->numIndices () &&
i2 < hull->numIndices () &&
i3 < hull->numIndices ());
int index1 = idx[i1];
int index2 = idx[i2];
int index3 = idx[i3];
btAssert(index1 < hull->numVertices () &&
index2 < hull->numVertices () &&
index3 < hull->numVertices ());
btVector3 v1 = vtx[index1];
btVector3 v2 = vtx[index2];
btVector3 v3 = vtx[index3];
btVector3 normal = (v3-v1).cross(v2-v1);
normal.normalize ();
glNormal3f(normal.getX(),normal.getY(),normal.getZ());
glVertex3f (v1.x(), v1.y(), v1.z());
glVertex3f (v2.x(), v2.y(), v2.z());
glVertex3f (v3.x(), v3.y(), v3.z());
}
glEnd ();
}
}
}
}
}
}
glNormal3f(0,1,0);
/// for polyhedral shapes
if (debugMode==btIDebugDraw::DBG_DrawFeaturesText && (shape->isPolyhedral()))
{
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
{
glColor3f(1.f, 1.f, 1.f);
int i;
for (i=0;i<polyshape->getNumVertices();i++)
{
btVector3 vtx;
polyshape->getVertex(i,vtx);
char buf[12];
sprintf(buf," %d",i);
//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
}
for (i=0;i<polyshape->getNumPlanes();i++)
{
btVector3 normal;
btVector3 vtx;
polyshape->getPlane(normal,vtx,i);
//btScalar d = vtx.dot(normal);
//char buf[12];
//sprintf(buf," plane %d",i);
//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
}
}
}
}
bool MyGetOpenFileName(HWND hwnd, LPCWSTR title,
LPCWSTR initialDir,
LPCWSTR filePath,
LPCWSTR filterDescription,
LPCWSTR filter,
UString &resPath
#ifdef UNDER_CE
, bool openFolder
#endif
)
{
const unsigned kBufSize = MAX_PATH * 2;
const unsigned kFilterBufSize = MAX_PATH;
if (!filter)
filter = L"*.*";
#ifndef _UNICODE
if (!g_IsNT)
{
CHAR buf[kBufSize];
MyStringCopy(buf, (const char *)GetSystemString(filePath));
// OPENFILENAME_NT4A
OPENFILENAMEA p;
memset(&p, 0, sizeof(p));
p.lStructSize = my_compatib_OPENFILENAMEA_size;
p.hwndOwner = hwnd;
CHAR filterBuf[kFilterBufSize];
{
CDoubleZeroStringListA dz(filterBuf, kFilterBufSize);
dz.Add(GetSystemString(filterDescription ? filterDescription : filter));
dz.Add(GetSystemString(filter));
dz.Finish();
p.lpstrFilter = filterBuf;
p.nFilterIndex = 1;
}
p.lpstrFile = buf;
p.nMaxFile = kBufSize;
CONV_U_To_A(p.lpstrInitialDir, initialDir, initialDirA);
CONV_U_To_A(p.lpstrTitle, title, titleA);
p.Flags = OFN_EXPLORER | OFN_HIDEREADONLY;
bool res = BOOLToBool(::GetOpenFileNameA(&p));
resPath = GetUnicodeString(buf);
return res;
}
else
#endif
{
WCHAR buf[kBufSize];
MyStringCopy(buf, filePath);
// OPENFILENAME_NT4W
OPENFILENAMEW p;
memset(&p, 0, sizeof(p));
p.lStructSize = my_compatib_OPENFILENAMEW_size;
p.hwndOwner = hwnd;
WCHAR filterBuf[kFilterBufSize];
{
CDoubleZeroStringListW dz(filterBuf, kFilterBufSize);
dz.Add(filterDescription ? filterDescription : filter);
dz.Add(filter);
dz.Finish();
p.lpstrFilter = filterBuf;
p.nFilterIndex = 1;
}
p.lpstrFile = buf;
p.nMaxFile = kBufSize;
p.lpstrInitialDir = initialDir;
p.lpstrTitle = title;
p.Flags = OFN_EXPLORER | OFN_HIDEREADONLY
#ifdef UNDER_CE
| (openFolder ? (MY__OFN_PROJECT | MY__OFN_SHOW_ALL) : 0)
#endif
;
bool res = BOOLToBool(::GetOpenFileNameW(&p));
resPath = buf;
return res;
}
}
void
WellsManager::init(const Opm::EclipseStateConstPtr eclipseState,
const size_t timeStep,
int number_of_cells,
const int* global_cell,
const int* cart_dims,
int dimensions,
const C2F& cell_to_faces,
FC begin_face_centroids,
const double* permeability)
{
if (dimensions != 3) {
OPM_THROW(std::runtime_error,
"We cannot initialize wells from a deck unless "
"the corresponding grid is 3-dimensional.");
}
if (eclipseState->getSchedule()->numWells() == 0) {
OPM_MESSAGE("No wells specified in Schedule section, "
"initializing no wells");
return;
}
std::map<int,int> cartesian_to_compressed;
setupCompressedToCartesian(global_cell, number_of_cells,
cartesian_to_compressed);
// Obtain phase usage data.
PhaseUsage pu = phaseUsageFromDeck(eclipseState);
// These data structures will be filled in this constructor,
// then used to initialize the Wells struct.
std::vector<std::string> well_names;
std::vector<WellData> well_data;
// For easy lookup:
std::map<std::string, int> well_names_to_index;
ScheduleConstPtr schedule = eclipseState->getSchedule();
std::vector<WellConstPtr> wells = schedule->getWells(timeStep);
std::vector<int> wells_on_proc;
well_names.reserve(wells.size());
well_data.reserve(wells.size());
typedef GridPropertyAccess::ArrayPolicy::ExtractFromDeck<double> DoubleArray;
typedef GridPropertyAccess::Compressed<DoubleArray, GridPropertyAccess::Tag::NTG> NTGArray;
DoubleArray ntg_glob(eclipseState, "NTG", 1.0);
NTGArray ntg(ntg_glob, global_cell);
EclipseGridConstPtr eclGrid = eclipseState->getEclipseGrid();
// use cell thickness (dz) from eclGrid
// dz overwrites values calculated by WellDetails::getCubeDim
std::vector<double> dz(number_of_cells);
{
std::vector<int> gc = compressedToCartesian(number_of_cells, global_cell);
for (int cell = 0; cell < number_of_cells; ++cell) {
dz[cell] = eclGrid->getCellThicknes(gc[cell]);
}
}
createWellsFromSpecs(wells, timeStep, cell_to_faces,
cart_dims,
begin_face_centroids,
dimensions,
dz,
well_names, well_data, well_names_to_index,
pu, cartesian_to_compressed, permeability, ntg,
wells_on_proc);
setupWellControls(wells, timeStep, well_names, pu, wells_on_proc);
{
GroupTreeNodeConstPtr fieldNode =
schedule->getGroupTree(timeStep)->getNode("FIELD");
GroupConstPtr fieldGroup =
schedule->getGroup(fieldNode->name());
well_collection_.addField(fieldGroup, timeStep, pu);
addChildGroups(fieldNode, schedule, timeStep, pu);
}
for (auto w = wells.begin(), e = wells.end(); w != e; ++w) {
well_collection_.addWell(*w, timeStep, pu);
}
well_collection_.setWellsPointer(w_);
well_collection_.applyGroupControls();
setupGuideRates(wells, timeStep, well_data, well_names_to_index);
// Debug output.
#define EXTRA_OUTPUT
#ifdef EXTRA_OUTPUT
/*
std::cout << "\t WELL DATA" << std::endl;
for(int i = 0; i< num_wells; ++i) {
std::cout << i << ": " << well_data[i].type << " "
<< well_data[i].control << " " << well_data[i].target
<< std::endl;
}
std::cout << "\n\t PERF DATA" << std::endl;
for(int i=0; i< int(wellperf_data.size()); ++i) {
for(int j=0; j< int(wellperf_data[i].size()); ++j) {
std::cout << i << ": " << wellperf_data[i][j].cell << " "
<< wellperf_data[i][j].well_index << std::endl;
}
}
*/
#endif
}
Vector3D Segment3D::as_vector() {
return Vector3D(dx(), dy(), dz() );
};
/*!
\return
true if the line is vertical, that is component on Z axes > of GM_NULL_TOLERANCE and the components on X and Y
axes < of GM_NULL_TOLERANCE, false otherwise
*/
bool GM_3dLine::isVertical() const {
if (fabs(dx()) < GM_NULL_TOLERANCE && fabs(dy()) < GM_NULL_TOLERANCE && fabs(dz()) > GM_NULL_TOLERANCE)
return true;
else
return false;
}
/*!
\return
true if the line is horizontal, that is component on Z axes < GM_NULL_TOLERANCE, false otherwise
*/
bool GM_3dLine::isHorizontal() const {
if (fabs(dz()) < GM_NULL_TOLERANCE)
return true;
else
return false;
}
void shCamera::tick(float secs)
{
// Set cursor:
Cursor = Target->P;
Cursor.Z += 1.5f;
// Speed:
float moveSpeed = 1 * Dist * secs;
float rotationSpeed = 2 * secs;
float zoomSpeed = 2 * Dist * secs;
// Dist:
Dist += (UserZoom[0] + UserZoom[1]) * zoomSpeed;
Dist = (Dist < 0.2f ? 0.2f : Dist);
// Rotate:
AngleX += (UserRotateX[0] + UserRotateX[1]) * rotationSpeed;
AngleY += (UserRotateY[0] + UserRotateY[1]) * rotationSpeed;
float maxX = 3.14159265f / 2.001f;
AngleX = (AngleX > +maxX ? +maxX : AngleX);
AngleX = (AngleX < -maxX ? -maxX : AngleX);
// Move:
enF3 dx(1, 0, 0);
enF3 dy(0, 1, 0);
enF3 dz(0, 0, 1);
dx.rotate(enF3(0, 1, 0), AngleY);
dx.normalizeXZ();
dz.rotate(enF3(0, 1, 0), AngleY);
dz.normalizeXZ();
Cursor.scaleAdd(dx, (UserMoveX[0] + UserMoveX[1]) * moveSpeed);
Cursor.scaleAdd(dy, (UserMoveY[0] + UserMoveY[1]) * moveSpeed);
Cursor.scaleAdd(dz, (UserMoveZ[0] + UserMoveZ[1]) * moveSpeed);
// Set matrix:
enMatrix mat;
enF3 from;
from.set(0, 0, - Dist);
from.rotate(enF3(-1, 0, 0), AngleX);
from.rotate(enF3( 0, 1, 0), AngleY);
from.add(Cursor);
mat.setLookAt(from, Cursor);
Camera->setMatrix(mat);
// Set entity:
mat.setIdentity();
mat.P = Cursor;
}
bool Cylinder::Intersect(const Ray &r, Float *tHit, SurfaceInteraction *isect,
bool testAlphaTexture) const {
Float phi;
Point3f pHit;
// Transform _Ray_ to object space
Vector3f oErr, dErr;
Ray ray = (*WorldToObject)(r, &oErr, &dErr);
// Compute quadratic cylinder coefficients
// Initialize _EFloat_ ray coordinate values
EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
EFloat a = dx * dx + dy * dy;
EFloat b = 2 * (dx * ox + dy * oy);
EFloat c = ox * ox + oy * oy - EFloat(radius) * EFloat(radius);
// Solve quadratic equation for _t_ values
EFloat t0, t1;
if (!Quadratic(a, b, c, &t0, &t1)) return false;
// Check quadric shape _t0_ and _t1_ for nearest intersection
if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
EFloat tShapeHit = t0;
if (tShapeHit.LowerBound() <= 0) {
tShapeHit = t1;
if (tShapeHit.UpperBound() > ray.tMax) return false;
}
// Compute cylinder hit point and $\phi$
pHit = ray((Float)tShapeHit);
// Refine cylinder intersection point
Float hitRad = std::sqrt(pHit.x * pHit.x + pHit.y * pHit.y);
pHit.x *= radius / hitRad;
pHit.y *= radius / hitRad;
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0) phi += 2 * Pi;
// Test cylinder intersection against clipping parameters
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) {
if (tShapeHit == t1) return false;
tShapeHit = t1;
if (t1.UpperBound() > ray.tMax) return false;
// Compute cylinder hit point and $\phi$
pHit = ray((Float)tShapeHit);
// Refine cylinder intersection point
Float hitRad = std::sqrt(pHit.x * pHit.x + pHit.y * pHit.y);
pHit.x *= radius / hitRad;
pHit.y *= radius / hitRad;
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0) phi += 2 * Pi;
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false;
}
// Find parametric representation of cylinder hit
Float u = phi / phiMax;
Float v = (pHit.z - zMin) / (zMax - zMin);
// Compute cylinder $\dpdu$ and $\dpdv$
Vector3f dpdu(-phiMax * pHit.y, phiMax * pHit.x, 0);
Vector3f dpdv(0, 0, zMax - zMin);
// Compute cylinder $\dndu$ and $\dndv$
Vector3f d2Pduu = -phiMax * phiMax * Vector3f(pHit.x, pHit.y, 0);
Vector3f d2Pduv(0, 0, 0), d2Pdvv(0, 0, 0);
// Compute coefficients for fundamental forms
Float E = Dot(dpdu, dpdu);
Float F = Dot(dpdu, dpdv);
Float G = Dot(dpdv, dpdv);
Vector3f N = Normalize(Cross(dpdu, dpdv));
Float e = Dot(N, d2Pduu);
Float f = Dot(N, d2Pduv);
Float g = Dot(N, d2Pdvv);
// Compute $\dndu$ and $\dndv$ from fundamental form coefficients
Float invEGF2 = 1 / (E * G - F * F);
Normal3f dndu = Normal3f((f * F - e * G) * invEGF2 * dpdu +
(e * F - f * E) * invEGF2 * dpdv);
Normal3f dndv = Normal3f((g * F - f * G) * invEGF2 * dpdu +
(f * F - g * E) * invEGF2 * dpdv);
// Compute error bounds for cylinder intersection
Vector3f pError = gamma(3) * Abs(Vector3f(pHit.x, pHit.y, 0));
// Initialize _SurfaceInteraction_ from parametric information
*isect = (*ObjectToWorld)(SurfaceInteraction(pHit, pError, Point2f(u, v),
-ray.d, dpdu, dpdv, dndu, dndv,
ray.time, this));
// Update _tHit_ for quadric intersection
*tHit = (Float)tShapeHit;
return true;
}
void cnbint(
int i,
const double pos[][3],
const double vel[][3],
const double mass[],
int nnb,
int list[],
double f[3],
double fdot[3]){
const int NBMAX = 512;
assert(nnb <= NBMAX);
float xbuf[NBMAX] __attribute__ ((aligned(16)));
float ybuf[NBMAX] __attribute__ ((aligned(16)));
float zbuf[NBMAX] __attribute__ ((aligned(16)));
float vxbuf[NBMAX] __attribute__ ((aligned(16)));
float vybuf[NBMAX] __attribute__ ((aligned(16)));
float vzbuf[NBMAX] __attribute__ ((aligned(16)));
float mbuf[NBMAX] __attribute__ ((aligned(16)));
assert((unsigned long)xbuf % 16 == 0);
double xi = pos[i][0];
double yi = pos[i][1];
double zi = pos[i][2];
float vxi = vel[i][0];
float vyi = vel[i][1];
float vzi = vel[i][2];
for(int k=0; k<nnb; k++){
int j = list[k];
#if 1
int jj = list[k+4];
__builtin_prefetch(pos[jj]);
__builtin_prefetch(pos[jj]+2);
__builtin_prefetch(vel[jj]);
__builtin_prefetch(vel[jj]+2);
__builtin_prefetch(&mass[jj]);
#endif
#if 0
xbuf[k] = pos[j][0] - xi;
ybuf[k] = pos[j][1] - yi;
zbuf[k] = pos[j][2] - zi;
vxbuf[k] = vel[j][0] - vxi;
vybuf[k] = vel[j][1] - vyi;
vzbuf[k] = vel[j][2] - vzi;
mbuf[k] = mass[j];
#else
double xj = pos[j][0];
double yj = pos[j][1];
double zj = pos[j][2];
float vxj = vel[j][0];
float vyj = vel[j][1];
float vzj = vel[j][2];
float mj = mass[j];
xj -= xi;
yj -= yi;
zj -= zi;
vxj -= vxi;
vyj -= vyi;
vzj -= vzi;
xbuf[k] = xj;
ybuf[k] = yj;
zbuf[k] = zj;
vxbuf[k] = vxj;
vybuf[k] = vyj;
vzbuf[k] = vzj;
mbuf[k] = mj;
#endif
}
for(int k=nnb; k%4; k++){
xbuf[k] = 16.0f;
ybuf[k] = 16.0f;
zbuf[k] = 16.0f;
vxbuf[k] = 0.0f;
vybuf[k] = 0.0f;
vzbuf[k] = 0.0f;
mbuf[k] = 0.0f;
}
v4df ax(0.0), ay(0.0), az(0.0);
v4sf jx(0.0), jy(0.0), jz(0.0);
for(int k=0; k<nnb; k+=4){
v4sf dx(xbuf + k);
v4sf dy(ybuf + k);
v4sf dz(zbuf + k);
v4sf dvx(vxbuf + k);
v4sf dvy(vybuf + k);
v4sf dvz(vzbuf + k);
v4sf mj(mbuf + k);
v4sf r2 = dx*dx + dy*dy + dz*dz;
v4sf rv = dx*dvx + dy*dvy + dz*dvz;
rv *= v4sf(-3.0f);
// v4sf rinv1 = r2.rsqrt() & v4sf(mask);
#if 1
v4sf rinv1 = r2.rsqrt();
#else
v4sf rinv1 = v4sf(v4df(1.0) / v4df(r2).sqrt());
#endif
v4sf rinv2 = rinv1 * rinv1;
rv *= rinv2;
v4sf rinv3 = mj * rinv1 * rinv2;
dx *= rinv3; ax += v4df(dx);
dy *= rinv3; ay += v4df(dy);
dz *= rinv3; az += v4df(dz);
dvx *= rinv3; jx += dvx;
dvy *= rinv3; jy += dvy;
dvz *= rinv3; jz += dvz;
dx *= rv; jx += dx;
dy *= rv; jy += dy;
dz *= rv; jz += dz;
}
f[0] = ax.sum();
f[1] = ay.sum();
f[2] = az.sum();
fdot[0] = (v4df(jx)).sum();
fdot[1] = (v4df(jy)).sum();
fdot[2] = (v4df(jz)).sum();
assert(f[0] == f[0]);
assert(f[1] == f[1]);
assert(f[2] == f[2]);
assert(fdot[0] == fdot[0]);
assert(fdot[1] == fdot[1]);
assert(fdot[2] == fdot[2]);
}
bool link_poly(
size_t size ,
size_t repeat ,
CppAD::vector<double> &a , // coefficients of polynomial
CppAD::vector<double> &z , // polynomial argument value
CppAD::vector<double> &ddp ) // second derivative w.r.t z
{
// speed test global option values
if( global_atomic )
return false;
// -----------------------------------------------------
// setup
typedef CppAD::AD<double> ADScalar;
typedef CppAD::vector<ADScalar> ADVector;
size_t i; // temporary index
size_t m = 1; // number of dependent variables
size_t n = 1; // number of independent variables
ADVector Z(n); // AD domain space vector
ADVector P(m); // AD range space vector
// choose the polynomial coefficients
CppAD::uniform_01(size, a);
// AD copy of the polynomial coefficients
ADVector A(size);
for(i = 0; i < size; i++)
A[i] = a[i];
// forward mode first and second differentials
CppAD::vector<double> p(1), dp(1), dz(1), ddz(1);
dz[0] = 1.;
ddz[0] = 0.;
// AD function object
CppAD::ADFun<double> f;
// --------------------------------------------------------------------
if( ! global_onetape ) while(repeat--)
{
// choose an argument value
CppAD::uniform_01(1, z);
Z[0] = z[0];
// declare independent variables
Independent(Z);
// AD computation of the function value
P[0] = CppAD::Poly(0, A, Z[0]);
// create function object f : A -> detA
f.Dependent(Z, P);
if( global_optimize )
f.optimize();
// skip comparison operators
f.compare_change_count(0);
// pre-allocate memory for three forward mode calculations
f.capacity_order(3);
// evaluate the polynomial
p = f.Forward(0, z);
// evaluate first order Taylor coefficient
dp = f.Forward(1, dz);
// second derivative is twice second order Taylor coef
ddp = f.Forward(2, ddz);
ddp[0] *= 2.;
}
else
{
// choose an argument value
CppAD::uniform_01(1, z);
Z[0] = z[0];
// declare independent variables
Independent(Z);
// AD computation of the function value
P[0] = CppAD::Poly(0, A, Z[0]);
// create function object f : A -> detA
f.Dependent(Z, P);
if( global_optimize )
f.optimize();
// skip comparison operators
f.compare_change_count(0);
while(repeat--)
{ // sufficient memory is allocated by second repetition
// get the next argument value
CppAD::uniform_01(1, z);
// evaluate the polynomial at the new argument value
p = f.Forward(0, z);
// evaluate first order Taylor coefficient
dp = f.Forward(1, dz);
// second derivative is twice second order Taylor coef
ddp = f.Forward(2, ddz);
ddp[0] *= 2.;
}
}
return true;
}
