void testCG(){ int N = 100; //read n=100 spd matrix from file float A[N*N]; readMatrix(A, N); //run cg on it to see if it converges //When porting cg to opencl only the mtx-vec multiply and the //dot-product(reduction) are run on the GPU everything is on the GPU int i = 0; int imax = 1000; float tol = 0.000001f; float r[N]; float b[N]; float d[N]; float x[N]; float q[N]; for(int i = 0; i < N; i++){ x[i] = 0.0f; b[i] = rand()/(float)RAND_MAX * 100.0f; r[i] = b[i]; d[i] = r[i]; } float rnew = dot(r,r,N); float rold = 0.0f; float r0 = rnew; //while(i < imax && rnew > 0.0000001*r0) { while(i < imax && rnew > tol) { mtx_times_vec(q,A,d,N); float alpha = rnew/(dot(d,q,N)); for(int j = 0; j < N; j++){ x[j] += alpha*d[j]; } for(int j = 0; j < N; j++){ r[j] -= alpha*q[j]; } rold = rnew; rnew = dot(r,r,N); float beta = rnew/rold; for(int j = 0; j < N; j++){ d[j] = r[j] + beta*d[j]; } i++; } //Check the answer float ax[N]; int goodMatrix = 1; mtx_times_vec(ax,A,x,N); for(int i = 0; i < N; i++){ float diff = ax[i] - b[i]; if(fabs(diff) > 0.01f){ goodMatrix = 0; printf("CG Result mismatch at idx %d ax[%d] = %3.6f != b[%d] = %3.6f\n", i,i,ax[i],i,b[i]); } } if(goodMatrix){ printf("CG Converged and Solved Ax=b\n"); }else{ printf("CG did not solve Ax=b\n"); } printf("CG Terminated with iterations %d, and rnew %3.6f\n",i, rnew); }
/// Helper function: reflect \c wi with respect to a given surface normal inline Vector reflect(const Vector &wi, const Normal &m) const { return 2 * dot(wi, m) * Vector(m) - wi; }
static double _simplex_noise(double xin, double yin, double zin) { double n0, n1, n2, n3; // Noise contributions from the four corners // Skew the input space to determine which simplex cell we're in double F3 = 1.0/3.0; double s = (xin+yin+zin)*F3; // Very nice and simple skew factor for 3D int i = FASTFLOOR(xin+s); int j = FASTFLOOR(yin+s); int k = FASTFLOOR(zin+s); double G3 = 1.0/6.0; // Very nice and simple unskew factor, too double t = (i+j+k)*G3; double X0 = i-t; // Unskew the cell origin back to (x,y,z) space double Y0 = j-t; double Z0 = k-t; double x0 = xin-X0; // The x,y,z distances from the cell origin double y0 = yin-Y0; double z0 = zin-Z0; // For the 3D case, the simplex shape is a slightly irregular tetrahedron. // Determine which simplex we are in. int i1, j1, k1; // Offsets for second corner of simplex in (i,j,k) coords int i2, j2, k2; // Offsets for third corner of simplex in (i,j,k) coords if(x0>=y0) { if(y0>=z0) { i1=1; // X Y Z order j1=0; k1=0; i2=1; j2=1; k2=0; } else if(x0>=z0) { i1=1; // X Z Y order j1=0; k1=0; i2=1; j2=0; k2=1; } else { i1=0; // Z X Y order j1=0; k1=1; i2=1; j2=0; k2=1; } } else // x0<y0 { if(y0<z0) { i1=0; // Z Y X order j1=0; k1=1; i2=0; j2=1; k2=1; } else if(x0<z0) { i1=0; // Y Z X order j1=1; k1=0; i2=0; j2=1; k2=1; } else { i1=0; // Y X Z order j1=1; k1=0; i2=1; j2=1; k2=0; } } // A step of (1,0,0) in (i,j,k) means a step of (1-c,-c,-c) in (x,y,z), // a step of (0,1,0) in (i,j,k) means a step of (-c,1-c,-c) in (x,y,z), and // a step of (0,0,1) in (i,j,k) means a step of (-c,-c,1-c) in (x,y,z), where // c = 1/6. double x1 = x0 - i1 + G3; // Offsets for second corner in (x,y,z) coords double y1 = y0 - j1 + G3; double z1 = z0 - k1 + G3; double x2 = x0 - i2 + 2.0*G3; // Offsets for third corner in (x,y,z) coords double y2 = y0 - j2 + 2.0*G3; double z2 = z0 - k2 + 2.0*G3; double x3 = x0 - 1.0 + 3.0*G3; // Offsets for last corner in (x,y,z) coords double y3 = y0 - 1.0 + 3.0*G3; double z3 = z0 - 1.0 + 3.0*G3; // Work out the hashed gradient indices of the four simplex corners int ii = i & 255; int jj = j & 255; int kk = k & 255; int gi0 = perm[ii+perm[jj+perm[kk]]] % 12; int gi1 = perm[ii+i1+perm[jj+j1+perm[kk+k1]]] % 12; int gi2 = perm[ii+i2+perm[jj+j2+perm[kk+k2]]] % 12; int gi3 = perm[ii+1+perm[jj+1+perm[kk+1]]] % 12; // Calculate the contribution from the four corners double t0 = 0.6 - x0*x0 - y0*y0 - z0*z0; if(t0<0) n0 = 0.0; else { t0 *= t0; n0 = t0 * t0 * dot(grad3[gi0], x0, y0, z0); } double t1 = 0.6 - x1*x1 - y1*y1 - z1*z1; if(t1<0) n1 = 0.0; else { t1 *= t1; n1 = t1 * t1 * dot(grad3[gi1], x1, y1, z1); } double t2 = 0.6 - x2*x2 - y2*y2 - z2*z2; if(t2<0) n2 = 0.0; else { t2 *= t2; n2 = t2 * t2 * dot(grad3[gi2], x2, y2, z2); } double t3 = 0.6 - x3*x3 - y3*y3 - z3*z3; if(t3<0) n3 = 0.0; else { t3 *= t3; n3 = t3 * t3 * dot(grad3[gi3], x3, y3, z3); } // Add contributions from each corner to get the final noise value. // The result is scaled to stay just inside [-1,1] return 32.0*(n0 + n1 + n2 + n3); }
// 2D raw Simplex noise double raw_noise_2d( const double x, const double y ) { // Noise contributions from the three corners double n0, n1, n2; // Skew the input space to determine which simplex cell we're in double F2 = 0.5 * (sqrt(3.0) - 1.0); // Hairy factor for 2D double s = (x + y) * F2; int i = fastfloor( x + s ); int j = fastfloor( y + s ); double G2 = (3.0 - sqrt(3.0)) / 6.0; double t = (i + j) * G2; // Unskew the cell origin back to (x,y) space double X0 = i-t; double Y0 = j-t; // The x,y distances from the cell origin double x0 = x-X0; double y0 = y-Y0; // For the 2D case, the simplex shape is an equilateral triangle. // Determine which simplex we are in. int i1, j1; // Offsets for second (middle) corner of simplex in (i,j) coords if(x0>y0) {i1=1; j1=0;} // lower triangle, XY order: (0,0)->(1,0)->(1,1) else {i1=0; j1=1;} // upper triangle, YX order: (0,0)->(0,1)->(1,1) // A step of (1,0) in (i,j) means a step of (1-c,-c) in (x,y), and // a step of (0,1) in (i,j) means a step of (-c,1-c) in (x,y), where // c = (3-sqrt(3))/6 double x1 = x0 - i1 + G2; // Offsets for middle corner in (x,y) unskewed coords double y1 = y0 - j1 + G2; double x2 = x0 - 1.0 + 2.0 * G2; // Offsets for last corner in (x,y) unskewed coords double y2 = y0 - 1.0 + 2.0 * G2; // Work out the hashed gradient indices of the three simplex corners int ii = i & 255; int jj = j & 255; int gi0 = perm[ii+perm[jj]] % 12; int gi1 = perm[ii+i1+perm[jj+j1]] % 12; int gi2 = perm[ii+1+perm[jj+1]] % 12; // Calculate the contribution from the three corners double t0 = 0.5 - x0*x0-y0*y0; if(t0<0) n0 = 0.0; else { t0 *= t0; n0 = t0 * t0 * dot(grad3[gi0], x0, y0); // (x,y) of grad3 used for 2D gradient } double t1 = 0.5 - x1*x1-y1*y1; if(t1<0) n1 = 0.0; else { t1 *= t1; n1 = t1 * t1 * dot(grad3[gi1], x1, y1); } double t2 = 0.5 - x2*x2-y2*y2; if(t2<0) n2 = 0.0; else { t2 *= t2; n2 = t2 * t2 * dot(grad3[gi2], x2, y2); } // Add contributions from each corner to get the final noise value. // The result is scaled to return values in the interval [-1,1]. return 70.0 * (n0 + n1 + n2); }
//----------------------------------------------------------------------------- // LLKeyframeStandMotion::onUpdate() //----------------------------------------------------------------------------- BOOL LLKeyframeStandMotion::onUpdate(F32 time, U8* joint_mask) { //------------------------------------------------------------------------- // let the base class update the cycle //------------------------------------------------------------------------- BOOL status = LLKeyframeMotion::onUpdate(time, joint_mask); if (!status) { return FALSE; } LLVector3 root_world_pos = mPelvisState->getJoint()->getParent()->getWorldPosition(); // have we received a valid world position for this avatar? if (root_world_pos.isExactlyZero()) { return TRUE; } //------------------------------------------------------------------------- // Stop tracking (start locking) ankles once ease in is done. // Setting this here ensures we track until we get valid foot position. //------------------------------------------------------------------------- if (dot(mPelvisState->getJoint()->getWorldRotation(), mLastGoodPelvisRotation) < ROTATION_THRESHOLD) { mLastGoodPelvisRotation = mPelvisState->getJoint()->getWorldRotation(); mLastGoodPelvisRotation.normalize(); mTrackAnkles = TRUE; } else if ((mCharacter->getCharacterPosition() - mLastGoodPosition).magVecSquared() > POSITION_THRESHOLD) { mLastGoodPosition = mCharacter->getCharacterPosition(); mTrackAnkles = TRUE; } else if (mPose.getWeight() < 1.f) { mTrackAnkles = TRUE; } //------------------------------------------------------------------------- // propagate joint positions to internal versions //------------------------------------------------------------------------- mPelvisJoint.setPosition( root_world_pos + mPelvisState->getPosition() ); mHipLeftJoint.setPosition( mHipLeftState->getJoint()->getPosition() ); mKneeLeftJoint.setPosition( mKneeLeftState->getJoint()->getPosition() ); mAnkleLeftJoint.setPosition( mAnkleLeftState->getJoint()->getPosition() ); mHipLeftJoint.setScale( mHipLeftState->getJoint()->getScale() ); mKneeLeftJoint.setScale( mKneeLeftState->getJoint()->getScale() ); mAnkleLeftJoint.setScale( mAnkleLeftState->getJoint()->getScale() ); mHipRightJoint.setPosition( mHipRightState->getJoint()->getPosition() ); mKneeRightJoint.setPosition( mKneeRightState->getJoint()->getPosition() ); mAnkleRightJoint.setPosition( mAnkleRightState->getJoint()->getPosition() ); mHipRightJoint.setScale( mHipRightState->getJoint()->getScale() ); mKneeRightJoint.setScale( mKneeRightState->getJoint()->getScale() ); mAnkleRightJoint.setScale( mAnkleRightState->getJoint()->getScale() ); //------------------------------------------------------------------------- // propagate joint rotations to internal versions //------------------------------------------------------------------------- mPelvisJoint.setRotation( mPelvisState->getJoint()->getWorldRotation() ); #if GO_TO_KEY_POSE mHipLeftJoint.setRotation( mHipLeftState->getRotation() ); mKneeLeftJoint.setRotation( mKneeLeftState->getRotation() ); mAnkleLeftJoint.setRotation( mAnkleLeftState->getRotation() ); mHipRightJoint.setRotation( mHipRightState->getRotation() ); mKneeRightJoint.setRotation( mKneeRightState->getRotation() ); mAnkleRightJoint.setRotation( mAnkleRightState->getRotation() ); #else mHipLeftJoint.setRotation( mHipLeftState->getJoint()->getRotation() ); mKneeLeftJoint.setRotation( mKneeLeftState->getJoint()->getRotation() ); mAnkleLeftJoint.setRotation( mAnkleLeftState->getJoint()->getRotation() ); mHipRightJoint.setRotation( mHipRightState->getJoint()->getRotation() ); mKneeRightJoint.setRotation( mKneeRightState->getJoint()->getRotation() ); mAnkleRightJoint.setRotation( mAnkleRightState->getJoint()->getRotation() ); #endif // need to wait for underlying keyframe motion to affect the skeleton if (mFrameNum == 2) { mIKLeft.setupJoints( &mHipLeftJoint, &mKneeLeftJoint, &mAnkleLeftJoint, &mTargetLeft ); mIKRight.setupJoints( &mHipRightJoint, &mKneeRightJoint, &mAnkleRightJoint, &mTargetRight ); } else if (mFrameNum < 2) { mFrameNum++; return TRUE; } mFrameNum++; //------------------------------------------------------------------------- // compute target position by projecting ankles to the ground //------------------------------------------------------------------------- if ( mTrackAnkles ) { mCharacter->getGround( mAnkleLeftJoint.getWorldPosition(), mPositionLeft, mNormalLeft); mCharacter->getGround( mAnkleRightJoint.getWorldPosition(), mPositionRight, mNormalRight); mTargetLeft.setPosition( mPositionLeft ); mTargetRight.setPosition( mPositionRight ); } //------------------------------------------------------------------------- // update solvers //------------------------------------------------------------------------- mIKLeft.solve(); mIKRight.solve(); //------------------------------------------------------------------------- // make ankle rotation conform to the ground //------------------------------------------------------------------------- if ( mTrackAnkles ) { LLVector4 dirLeft4 = mAnkleLeftJoint.getWorldMatrix().getFwdRow4(); LLVector4 dirRight4 = mAnkleRightJoint.getWorldMatrix().getFwdRow4(); LLVector3 dirLeft = vec4to3( dirLeft4 ); LLVector3 dirRight = vec4to3( dirRight4 ); LLVector3 up; LLVector3 dir; LLVector3 left; up = mNormalLeft; up.normVec(); if (mFlipFeet) { up *= -1.0f; } dir = dirLeft; dir.normVec(); left = up % dir; left.normVec(); dir = left % up; mRotationLeft = LLQuaternion( dir, left, up ); up = mNormalRight; up.normVec(); if (mFlipFeet) { up *= -1.0f; } dir = dirRight; dir.normVec(); left = up % dir; left.normVec(); dir = left % up; mRotationRight = LLQuaternion( dir, left, up ); } mAnkleLeftJoint.setWorldRotation( mRotationLeft ); mAnkleRightJoint.setWorldRotation( mRotationRight ); //------------------------------------------------------------------------- // propagate joint rotations to joint states //------------------------------------------------------------------------- mHipLeftState->setRotation( mHipLeftJoint.getRotation() ); mKneeLeftState->setRotation( mKneeLeftJoint.getRotation() ); mAnkleLeftState->setRotation( mAnkleLeftJoint.getRotation() ); mHipRightState->setRotation( mHipRightJoint.getRotation() ); mKneeRightState->setRotation( mKneeRightJoint.getRotation() ); mAnkleRightState->setRotation( mAnkleRightJoint.getRotation() ); //llinfos << "Stand drift amount " << (mCharacter->getCharacterPosition() - mLastGoodPosition).magVec() << llendl; // llinfos << "DEBUG: " << speed << " : " << mTrackAnkles << llendl; return TRUE; }
/* task that renders a single screen tile */ Vec3fa renderPixelStandard(float x, float y, const Vec3fa& vx, const Vec3fa& vy, const Vec3fa& vz, const Vec3fa& p) { float weight = 1.0f; Vec3fa color = Vec3fa(0.0f); /* initialize ray */ RTCRay2 primary; primary.org = p; primary.dir = normalize(x*vx + y*vy + vz); primary.tnear = 0.0f; primary.tfar = inf; primary.geomID = RTC_INVALID_GEOMETRY_ID; primary.primID = RTC_INVALID_GEOMETRY_ID; primary.mask = -1; primary.time = 0; primary.transparency = 0.0f; while (true) { /* intersect ray with scene */ rtcIntersect(g_scene,*((RTCRay*)&primary)); // FIXME: use (RTCRay&) cast /* shade pixels */ if (primary.geomID == RTC_INVALID_GEOMETRY_ID) break; float opacity = 1.0f-primary.transparency; Vec3fa diffuse = colors[primary.primID]; Vec3fa La = diffuse*0.5f; color = color + weight*opacity*La; // FIXME: += Vec3fa lightDir = normalize(Vec3fa(-1,-1,-1)); /* initialize shadow ray */ RTCRay2 shadow; shadow.org = primary.org + primary.tfar*primary.dir; shadow.dir = neg(lightDir); shadow.tnear = 0.001f; shadow.tfar = inf; shadow.geomID = RTC_INVALID_GEOMETRY_ID; shadow.primID = RTC_INVALID_GEOMETRY_ID; shadow.mask = -1; shadow.time = 0; shadow.transparency = 1.0f; /* trace shadow ray */ rtcOccluded(g_scene,*((RTCRay*)&shadow)); // FIXME: use (RTCRay&) cast /* add light contribution */ if (shadow.geomID) { Vec3fa Ll = diffuse*shadow.transparency*clamp(-dot(lightDir,normalize(primary.Ng)),0.0f,1.0f); color = color + weight*opacity*Ll; // FIXME: += } /* shoot transmission ray */ weight *= primary.transparency; primary.tnear = 1.001f*primary.tfar; primary.tfar = inf; primary.geomID = RTC_INVALID_GEOMETRY_ID; primary.primID = RTC_INVALID_GEOMETRY_ID; primary.transparency = 0.0f; } return color; }
/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // INTRODUCE THE VARIABLES: // ---------------------------- DifferentialState x("", 10, 1); // a differential state vector with dimension 10. (vector) DifferentialState y ; // another differential state y (scalar) Control u("", 2, 1); // a control input with dimension 2. (vector) Parameter p ; // a parameter (here a scalar). (scalar) DifferentialEquation f ; // the differential equation const double t_start = 0.0; const double t_end = 1.0; // READ A MATRIX "A" FROM A FILE: // ------------------------------ DMatrix A; A.read( "matrix_vector_ocp_A.txt" ); DMatrix B; B.read( "matrix_vector_ocp_B.txt" ); // READ A VECTOR "x0" FROM A FILE: // ------------------------------- DVector x0; x0.read( "matrix_vector_ocp_x0.txt" ); // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- f << dot(x) == -(A*x) + B*u; // matrix vector notation for a linear equation f << dot(y) == x.transpose()*x + 2.0*u.transpose()*u; // matrix vector notation: x^x = scalar product = ||x||_2^2 // u^u = scalar product = ||u||_2^2 // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( t_start, t_end, 20 ); ocp.minimizeMayerTerm( y ); ocp.subjectTo( f ); ocp.subjectTo( AT_START, x == x0 ); ocp.subjectTo( AT_START, y == 0.0 ); GnuplotWindow window; window.addSubplot( x(0),"x0" ); window.addSubplot( x(6),"x6" ); window.addSubplot( u(0),"u0" ); window.addSubplot( u(1),"u1" ); // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP: // --------------------------------------------------- OptimizationAlgorithm algorithm(ocp); algorithm.set( MAX_NUM_ITERATIONS, 20 ); algorithm.set( KKT_TOLERANCE, 1e-10 ); algorithm << window; algorithm.solve(); return 0; }
//================== Plane ============ Plane::Plane(const float3 &A,const float3 &B,const float3 &C) { normal = normalize( cross((B-A),(C-A)) ); d = -dot(normal, A); }
void DistanceRayToSegment(const Ray& ray, const LineSeg& segment, float* out_distTo, float* out_distBetween, float3* out_pos, float3* out_nor) { // Find closest distance between ray and segment: // Construct seg ray: // seg.direction = normal(segment.B - segment.A) // seg.pos = segment.A // // Construct plane through ray.pos that contains both ray.direction and segment.direction // Construct second plane through segment.pos that contains both ray.direction and segment.direction // The normal of both these planes will be: // planeNormal = normal(cross(ray.direction, seg.direction)) // planeR . planeNormal = ray.pos . planeNormal // planeS . planeNormal = seg.pos . planeNormal // // The distance between these planes will be ray-to-seg projected on the planeNormal: // distRayPosToSegment = (seg.pos - ray.pos) . planeNormal // // Now we need to find where this closest point occurs to return distanceToClosest and check if // it actually occurs between segment.A and segment.B: // rayLine = ray.pos + tRay * ray.direction // segLine = seg.pos + tSeg * seg.direction // // Since we already have the normal and distance between ray and segment: // rayLine + distBetweenRaySegment * planeNormal = segLine // // By setting components equal we can solve for tRay and tSeg: // (B * seg.direction.x + A * seg.direction.y) // tRay = --------------------------------------------------------------------------- // (ray.direction.y * seg.direction.x - ray.direction.x * seg.direction.y) // // where // A = ray.pos.x - seg.pos.x + distBetweenRaySegment * planeNormal.x // B = seg.pos.y - ray.pos.y - distBetweenRaySegment * planeNormal.y // // tSeg = (A + tRay * ray.direction.x) / seg.direction.x // // if tSeg < 0 => use point-to-ray-distance(segment.A, ray) // else if tSeg > length(segment.B - segment.A) => use point-to-ray-distance(segment.B, ray) // else => distToClosest = tRay // posClosest = tRay * ray.direction + ray.position // Ray seg(segment.A, segment.B - segment.A); // check for parallel seg and ray float3 planeNormal = normalize(cross(ray.direction, seg.direction)); float distBetweenRaySegment = dot(seg.pos - ray.pos, planeNormal); float A = ray.pos.x - seg.pos.x + distBetweenRaySegment * planeNormal.x; float B = seg.pos.y - ray.pos.y - distBetweenRaySegment * planeNormal.y; float tRay = (B * seg.direction.x + A * seg.direction.y) / (ray.direction.y * seg.direction.x - ray.direction.x * seg.direction.y); float tSeg = (A + tRay * ray.direction.x) / seg.direction.x; float segmentLength = length(segment.B - segment.A); if (tRay < 0) // the segment is behind us { DistancePointToSegment(segment, ray.pos, out_distTo, out_distBetween, out_pos, out_nor); } else if (tSeg < 0) // closest is before segment.A { DistanceRayToPoint(ray, segment.A, out_distTo, out_distBetween, out_pos, out_nor); } else if (tSeg > segmentLength) // closest is after segment.B { DistanceRayToPoint(ray, segment.B, out_distTo, out_distBetween, out_pos, out_nor); } else { *out_distTo = tRay; *out_distBetween = fabsf(distBetweenRaySegment); *out_pos = ray.pos + tRay * ray.direction; *out_nor = planeNormal; } }
inline Vector3D reflect(const Vector3D &v, const Vector3D &n) { return v - 2*dot(n, v)*n; }
int main( ){ USING_NAMESPACE_ACADO // INTRODUCE FIXED PARAMETERS: // --------------------------- #define v 0.1 #define L 1.0 #define Beta 0.2 #define Delta 0.25 #define E 11250.0 #define k0 1E+06 #define R 1.986 #define K1 250000.0 #define Cin 0.02 #define Tin 340.0 // INTRODUCE THE VARIABLES: // ------------------------- DifferentialState x1,x2; Control u ; DifferentialEquation f( 0.0, L ); // DEFINE A DIFFERENTIAL EQUATION: // ------------------------------- double Alpha, Gamma; Alpha = k0*exp(-E/(R*Tin)); Gamma = E/(R*Tin); f << dot(x1) == Alpha /v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2)); f << dot(x2) == (Alpha*Delta)/v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2)) + Beta/v * (u-x2); // DEFINE AN OPTIMAL CONTROL PROBLEM: // ---------------------------------- OCP ocp( 0.0, L, 50 ); ocp.minimizeMayerTerm( 0, Cin*(1.0-x1) ); // Solve conversion optimal problem ocp.minimizeMayerTerm( 1, (pow((Tin*x2),2.0)/K1) + 0.005*Cin*(1.0-x1) ); // Solve energy optimal problem (perturbed by small conversion cost; // otherwise the problem is ill-defined.) ocp.subjectTo( f ); ocp.subjectTo( AT_START, x1 == 0.0 ); ocp.subjectTo( AT_START, x2 == 0.0 ); ocp.subjectTo( 0.0 <= x1 <= 1.0 ); ocp.subjectTo( (280.0-Tin)/Tin <= x2 <= (400.0-Tin)/Tin ); ocp.subjectTo( (280.0-Tin)/Tin <= u <= (400.0-Tin)/Tin ); // DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE OCP: // ----------------------------------------------------- MultiObjectiveAlgorithm algorithm(ocp); algorithm.set( INTEGRATOR_TYPE, INT_BDF ); algorithm.set( KKT_TOLERANCE, 1e-7 ); algorithm.set( PARETO_FRONT_GENERATION , PFG_NORMAL_BOUNDARY_INTERSECTION ); algorithm.set( PARETO_FRONT_DISCRETIZATION, 11 ); // Minimize individual objective function algorithm.solveSingleObjective(0); // Minimize individual objective function algorithm.solveSingleObjective(1); // Generate Pareto set algorithm.solve(); algorithm.getWeights("plug_flow_reactor_nbi_weights.txt"); algorithm.getAllDifferentialStates("plug_flow_reactor_nbi_states.txt"); algorithm.getAllControls("plug_flow_reactor_nbi_controls.txt"); // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW: // ------------------------------------------ VariablesGrid paretoFront; algorithm.getParetoFront( paretoFront ); GnuplotWindow window1; window1.addSubplot( paretoFront, "Pareto Front (conversion versus energy)", "OUTLET CONCENTRATION", "ENERGY", PM_POINTS ); window1.plot( ); // PRINT INFORMATION ABOUT THE ALGORITHM: // -------------------------------------- algorithm.printInfo(); // SAVE INFORMATION: // ----------------- paretoFront.print(); return 0; }
// Returns "distance" between target destination and resulting xfrom F32 LLDrawable::updateXform(BOOL undamped) { BOOL damped = !undamped; // Position LLVector3 old_pos(mXform.getPosition()); LLVector3 target_pos; if (mXform.isRoot()) { // get root position in your agent's region target_pos = mVObjp->getPositionAgent(); } else { // parent-relative position target_pos = mVObjp->getPosition(); } // Rotation LLQuaternion old_rot(mXform.getRotation()); LLQuaternion target_rot = mVObjp->getRotation(); //scaling LLVector3 target_scale = mVObjp->getScale(); LLVector3 old_scale = mCurrentScale; // Damping F32 dist_squared = 0.f; F32 camdist2 = (mDistanceWRTCamera * mDistanceWRTCamera); if (damped && isVisible()) { F32 lerp_amt = llclamp(LLCriticalDamp::getInterpolant(OBJECT_DAMPING_TIME_CONSTANT), 0.f, 1.f); LLVector3 new_pos = lerp(old_pos, target_pos, lerp_amt); dist_squared = dist_vec_squared(new_pos, target_pos); LLQuaternion new_rot = nlerp(lerp_amt, old_rot, target_rot); dist_squared += (1.f - dot(new_rot, target_rot)) * 10.f; LLVector3 new_scale = lerp(old_scale, target_scale, lerp_amt); dist_squared += dist_vec_squared(new_scale, target_scale); if ((dist_squared >= MIN_INTERPOLATE_DISTANCE_SQUARED * camdist2) && (dist_squared <= MAX_INTERPOLATE_DISTANCE_SQUARED)) { // interpolate target_pos = new_pos; target_rot = new_rot; target_scale = new_scale; } else if (mVObjp->getAngularVelocity().isExactlyZero()) { // snap to final position (only if no target omega is applied) dist_squared = 0.0f; if (getVOVolume() && !isRoot()) { //child prim snapping to some position, needs a rebuild gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE); } } } if ((mCurrentScale != target_scale) || (!isRoot() && (dist_squared >= MIN_INTERPOLATE_DISTANCE_SQUARED || !mVObjp->getAngularVelocity().isExactlyZero() || target_pos != mXform.getPosition() || target_rot != mXform.getRotation()))) { //child prim moving or scale change requires immediate rebuild mCurrentScale = target_scale; gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE); } else if (!getVOVolume() && !isAvatar()) { movePartition(); } // Update mXform.setPosition(target_pos); mXform.setRotation(target_rot); mXform.setScale(LLVector3(1,1,1)); //no scale in drawable transforms (IT'S A RULE!) mXform.updateMatrix(); if (mSpatialBridge) { gPipeline.markMoved(mSpatialBridge, FALSE); } return dist_squared; }
unsigned int trace(const vec p, const vec d) { static const vec light = { 0.408248, 0.816497, -0.408248 }; vec o; int i; float t = 0, a = 0; for (i = 0; i < 75; i++) { mov(o, d); mul(o, t); add(o, p); if (f(o) < 0) break; a = t; t += 0.125; } if (i == 75) return 0x000000; float b = t; for (i = 0; i < 10; i++) { t = (a + b) * 0.5; mov(o, d); mul(o, t); add(o, p); if (f(o) < 0) b = t; else a = t; } mov(o, d); mul(o, t); add(o, p); vec o2, o3; mov(o2, o); mov(o3, o); o[0] += 0.1; o2[1] += 0.1; o3[2] += 0.1; vec n = { f(o), f(o2), f(o2) }; normalize(n); float diffuse = -dot(n, light); if (diffuse < 0) diffuse = 0; mul(n, dot(d, n) * -2); add(n, d); float specular = dot(n, light); if (specular < 0) specular = 0; specular = pow(specular, 60); float fog = 1 - t / 11; fog *= fog; float pattern = ( (fmod(o[0], 0.2) < 0.1) + (fmod(o[1], 0.2) < 0.1) + (fmod(o[2], 0.2) < 0.1)) * 50; return COLOR_RGB( (255 * diffuse + specular * 200) * fog, (pattern * diffuse + specular * 200) * fog, (130 * diffuse + specular * 200) * fog); }
Spectrum Li(const RayDifferential &r, RadianceQueryRecord &rRec) const { /* Some aliases and local variables */ const Scene *scene = rRec.scene; Intersection &its = rRec.its; RayDifferential ray(r); Spectrum Li(0.0f); /* Perform the first ray intersection (or ignore if the intersection has already been provided). */ rRec.rayIntersect(ray); ray.mint = Epsilon; Spectrum pathThroughput(1.0f); while (rRec.depth <= m_maxDepth || m_maxDepth < 0) { if (!its.isValid()) { /* If no intersection could be found, potentially return radiance from a background luminaire if it exists */ if (rRec.type & RadianceQueryRecord::EEmittedRadiance) Li += pathThroughput * scene->LeBackground(ray); break; } const BSDF *bsdf = its.getBSDF(ray); if (EXPECT_NOT_TAKEN(bsdf == NULL)) { /* The MI path tracer doesn't support surfaces without a BSDF (e.g. medium transitions) -- give up. */ break; } /* Possibly include emitted radiance if requested */ if (its.isLuminaire() && (rRec.type & RadianceQueryRecord::EEmittedRadiance)) Li += pathThroughput * its.Le(-ray.d); /* Include radiance from a subsurface integrator if requested */ if (its.hasSubsurface() && (rRec.type & RadianceQueryRecord::ESubsurfaceRadiance)) Li += pathThroughput * its.LoSub(scene, rRec.sampler, -ray.d, rRec.depth); if (m_maxDepth > 0 && rRec.depth >= m_maxDepth) break; /* ==================================================================== */ /* Luminaire sampling */ /* ==================================================================== */ /* Prevent light leaks due to the use of shading normals */ Float wiDotGeoN = -dot(its.geoFrame.n, ray.d), wiDotShN = Frame::cosTheta(its.wi); if (wiDotGeoN * wiDotShN < 0 && m_strictNormals) break; /* Estimate the direct illumination if this is requested */ LuminaireSamplingRecord lRec; if (rRec.type & RadianceQueryRecord::EDirectSurfaceRadiance && scene->sampleLuminaire(its.p, ray.time, lRec, rRec.nextSample2D())) { /* Allocate a record for querying the BSDF */ const Vector wo = -lRec.d; const BSDFQueryRecord bRec(its, its.toLocal(wo)); /* Evaluate BSDF * cos(theta) */ const Spectrum bsdfVal = bsdf->fCos(bRec); Float woDotGeoN = dot(its.geoFrame.n, wo); /* Prevent light leaks due to the use of shading normals */ if (!bsdfVal.isZero() && (!m_strictNormals || woDotGeoN * Frame::cosTheta(bRec.wo) > 0)) { /* Calculate prob. of having sampled that direction using BSDF sampling */ Float bsdfPdf = (lRec.luminaire->isIntersectable() || lRec.luminaire->isBackgroundLuminaire()) ? bsdf->pdf(bRec) : 0; /* Weight using the power heuristic */ const Float weight = miWeight(lRec.pdf, bsdfPdf); Li += pathThroughput * lRec.value * bsdfVal * weight; } } /* ==================================================================== */ /* BSDF sampling */ /* ==================================================================== */ /* Sample BSDF * cos(theta) */ BSDFQueryRecord bRec(its); Float bsdfPdf; Spectrum bsdfVal = bsdf->sampleCos(bRec, bsdfPdf, rRec.nextSample2D()); if (bsdfVal.isZero()) break; bsdfVal /= bsdfPdf; /* Prevent light leaks due to the use of shading normals */ const Vector wo = its.toWorld(bRec.wo); Float woDotGeoN = dot(its.geoFrame.n, wo); if (woDotGeoN * Frame::cosTheta(bRec.wo) <= 0 && m_strictNormals) break; /* Trace a ray in this direction */ ray = Ray(its.p, wo, ray.time); bool hitLuminaire = false; if (scene->rayIntersect(ray, its)) { /* Intersected something - check if it was a luminaire */ if (its.isLuminaire()) { lRec = LuminaireSamplingRecord(its, -ray.d); lRec.value = its.Le(-ray.d); hitLuminaire = true; } } else { /* No intersection found. Possibly, there is a background luminaire such as an environment map? */ if (scene->hasBackgroundLuminaire()) { lRec.luminaire = scene->getBackgroundLuminaire(); lRec.value = lRec.luminaire->Le(ray); lRec.d = -ray.d; hitLuminaire = true; } else { rRec.depth++; break; } } /* If a luminaire was hit, estimate the local illumination and weight using the power heuristic */ if (hitLuminaire && (rRec.type & RadianceQueryRecord::EDirectSurfaceRadiance)) { /* Prob. of having generated this sample using luminaire sampling */ const Float lumPdf = (!(bRec.sampledType & BSDF::EDelta)) ? scene->pdfLuminaire(ray.o, lRec) : 0; const Float weight = miWeight(bsdfPdf, lumPdf); Li += pathThroughput * lRec.value * bsdfVal * weight; } /* ==================================================================== */ /* Indirect illumination */ /* ==================================================================== */ /* Set the recursive query type */ /* Stop if no surface was hit by the BSDF sample or if indirect illumination was not requested */ if (!its.isValid() || !(rRec.type & RadianceQueryRecord::EIndirectSurfaceRadiance)) break; rRec.type = RadianceQueryRecord::ERadianceNoEmission; /* Russian roulette - Possibly stop the recursion. Don't do this when dealing with a transmission component, since solid angle compression factors cause problems with the heuristic below */ if (rRec.depth >= m_rrDepth && !(bRec.sampledType & BSDF::ETransmission)) { /* Assuming that BSDF importance sampling is perfect, 'bsdfVal.max()' should equal the maximum albedo over all spectral samples */ Float approxAlbedo = std::min((Float) 0.9f, bsdfVal.max()); if (rRec.nextSample1D() > approxAlbedo) break; else pathThroughput /= approxAlbedo; } pathThroughput *= bsdfVal; rRec.depth++; } /* Store statistics */ avgPathLength.incrementBase(); avgPathLength += rRec.depth; return Li; }
bool Path::AttemptSimplify(int off, int N, double treshhold, PathDescrCubicTo &res,int &worstP) { Geom::Point start; Geom::Point end; // pour une coordonnee double *Xk; // la coordonnee traitee (x puis y) double *Yk; // la coordonnee traitee (x puis y) double *lk; // les longueurs de chaque segment double *tk; // les tk double *Qk; // les Qk char *fk; // si point force Geom::Point cp1; Geom::Point cp2; if (N == 2) { worstP = 1; return true; } start = pts[off].p; cp1 = pts[off + 1].p; end = pts[off + N - 1].p; res.p = end; res.start[0] = res.start[1] = 0; res.end[0] = res.end[1] = 0; if (N == 3) { // start -> cp1 -> end res.start = cp1 - start; res.end = end - cp1; worstP = 1; return true; } // Totally inefficient, allocates & deallocates all the time. tk = (double *) g_malloc(N * sizeof(double)); Qk = (double *) g_malloc(N * sizeof(double)); Xk = (double *) g_malloc(N * sizeof(double)); Yk = (double *) g_malloc(N * sizeof(double)); lk = (double *) g_malloc(N * sizeof(double)); fk = (char *) g_malloc(N * sizeof(char)); // chord length method tk[0] = 0.0; lk[0] = 0.0; { Geom::Point prevP = start; for (int i = 1; i < N; i++) { Xk[i] = pts[off + i].p[Geom::X]; Yk[i] = pts[off + i].p[Geom::Y]; if ( pts[off + i].isMoveTo == polyline_forced ) { fk[i] = 0x01; } else { fk[i] = 0; } Geom::Point diff(Xk[i] - prevP[Geom::X], Yk[i] - prevP[1]); prevP[0] = Xk[i]; prevP[1] = Yk[i]; lk[i] = Geom::L2(diff); tk[i] = tk[i - 1] + lk[i]; } } if (tk[N - 1] < 0.00001) { // longueur nulle res.start[0] = res.start[1] = 0; res.end[0] = res.end[1] = 0; double worstD = 0; worstP = -1; for (int i = 1; i < N; i++) { Geom::Point nPt; bool isForced = fk[i]; nPt[0] = Xk[i]; nPt[1] = Yk[i]; double nle = DistanceToCubic(start, res, nPt); if ( isForced ) { // forced points are favored for splitting the recursion; we do this by increasing their distance if ( worstP < 0 || 2 * nle > worstD ) { worstP = i; worstD = 2 * nle; } } else { if ( worstP < 0 || nle > worstD ) { worstP = i; worstD = nle; } } } g_free(tk); g_free(Qk); g_free(Xk); g_free(Yk); g_free(fk); g_free(lk); return false; } double totLen = tk[N - 1]; for (int i = 1; i < N - 1; i++) { tk[i] /= totLen; } res.p = end; if ( FitCubic(start, res, Xk, Yk, Qk, tk, N) ) { cp1 = start + res.start / 3; cp2 = end + res.end / 3; } else { // aie, non-inversible res.start[0] = res.start[1] = 0; res.end[0] = res.end[1] = 0; double worstD = 0; worstP = -1; for (int i = 1; i < N; i++) { Geom::Point nPt(Xk[i], Yk[i]); bool isForced = fk[i]; double nle = DistanceToCubic(start, res, nPt); if ( isForced ) { // forced points are favored for splitting the recursion; we do this by increasing their distance if ( worstP < 0 || 2 * nle > worstD ) { worstP = i; worstD = 2 * nle; } } else { if ( worstP < 0 || nle > worstD ) { worstP = i; worstD = nle; } } } g_free(tk); g_free(Qk); g_free(Xk); g_free(Yk); g_free(fk); g_free(lk); return false; } // calcul du delta= pondere par les longueurs des segments double delta = 0; { double worstD = 0; worstP = -1; Geom::Point prevAppP; Geom::Point prevP; double prevDist; prevP[0] = Xk[0]; prevP[1] = Yk[0]; prevAppP = prevP; // le premier seulement prevDist = 0; #ifdef with_splotch_killer if ( N <= 20 ) { for (int i = 1; i < N - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; Geom::Point midAppP; Geom::Point midP; double midDist; curAppP[0] = N13 (tk[i]) * cp1[0] + N23 (tk[i]) * cp2[0] + N03 (tk[i]) * Xk[0] + N33 (tk[i]) * Xk[N - 1]; curAppP[1] = N13 (tk[i]) * cp1[1] + N23 (tk[i]) * cp2[1] + N03 (tk[i]) * Yk[0] + N33 (tk[i]) * Yk[N - 1]; curP[0] = Xk[i]; curP[1] = Yk[i]; midAppP[0] = N13 (0.5*(tk[i]+tk[i-1])) * cp1[0] + N23 (0.5*(tk[i]+tk[i-1])) * cp2[0] + N03 (0.5*(tk[i]+tk[i-1])) * Xk[0] + N33 (0.5*(tk[i]+tk[i-1])) * Xk[N - 1]; midAppP[1] = N13 (0.5*(tk[i]+tk[i-1])) * cp1[1] + N23 (0.5*(tk[i]+tk[i-1])) * cp2[1] + N03 (0.5*(tk[i]+tk[i-1])) * Yk[0] + N33 (0.5*(tk[i]+tk[i-1])) * Yk[N - 1]; midP=0.5*(curP+prevP); Geom::Point diff; diff = curAppP-curP; curDist = dot(diff,diff); diff = midAppP-midP; midDist = dot(diff,diff); delta+=0.3333*(curDist+prevDist+midDist)/**lk[i]*/; if ( curDist > worstD ) { worstD = curDist; worstP = i; } else if ( fk[i] && 2*curDist > worstD ) { worstD = 2*curDist; worstP = i; } prevP = curP; prevAppP = curAppP; prevDist = curDist; } delta/=totLen; } else { #endif for (int i = 1; i < N - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; curAppP[0] = N13 (tk[i]) * cp1[0] + N23 (tk[i]) * cp2[0] + N03 (tk[i]) * Xk[0] + N33 (tk[i]) * Xk[N - 1]; curAppP[1] = N13 (tk[i]) * cp1[1] + N23 (tk[i]) * cp2[1] + N03 (tk[i]) * Yk[0] + N33 (tk[i]) * Yk[N - 1]; curP[0] = Xk[i]; curP[1] = Yk[i]; Geom::Point diff; diff = curAppP-curP; curDist = dot(diff,diff); delta += curDist; if ( curDist > worstD ) { worstD = curDist; worstP = i; } else if ( fk[i] && 2*curDist > worstD ) { worstD = 2*curDist; worstP = i; } prevP = curP; prevAppP = curAppP; prevDist = curDist; } #ifdef with_splotch_killer } #endif } if (delta < treshhold * treshhold) { // premier jet res.start = 3.0 * (cp1 - start); res.end = -3.0 * (cp2 - end); res.p = end; // Refine a little. for (int i = 1; i < N - 1; i++) { Geom::Point pt; pt[0] = Xk[i]; pt[1] = Yk[i]; tk[i] = RaffineTk (pt, start, cp1, cp2, end, tk[i]); if (tk[i] < tk[i - 1]) { // Force tk to be monotonic non-decreasing. tk[i] = tk[i - 1]; } } if ( FitCubic(start,res,Xk,Yk,Qk,tk,N) ) { } else { // ca devrait jamais arriver, mais bon res.start = 3.0 * (cp1 - start); res.end = -3.0 * (cp2 - end); g_free(tk); g_free(Qk); g_free(Xk); g_free(Yk); g_free(fk); g_free(lk); return true; } double ndelta = 0; { double worstD = 0; worstP = -1; Geom::Point prevAppP; Geom::Point prevP; double prevDist; prevP[0] = Xk[0]; prevP[1] = Yk[0]; prevAppP = prevP; // le premier seulement prevDist = 0; #ifdef with_splotch_killer if ( N <= 20 ) { for (int i = 1; i < N - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; Geom::Point midAppP; Geom::Point midP; double midDist; curAppP[0] = N13 (tk[i]) * cp1[0] + N23 (tk[i]) * cp2[0] + N03 (tk[i]) * Xk[0] + N33 (tk[i]) * Xk[N - 1]; curAppP[1] = N13 (tk[i]) * cp1[1] + N23 (tk[i]) * cp2[1] + N03 (tk[i]) * Yk[0] + N33 (tk[i]) * Yk[N - 1]; curP[0] = Xk[i]; curP[1] = Yk[i]; midAppP[0] = N13 (0.5*(tk[i]+tk[i-1])) * cp1[0] + N23 (0.5*(tk[i]+tk[i-1])) * cp2[0] + N03 (0.5*(tk[i]+tk[i-1])) * Xk[0] + N33 (0.5*(tk[i]+tk[i-1])) * Xk[N - 1]; midAppP[1] = N13 (0.5*(tk[i]+tk[i-1])) * cp1[1] + N23 (0.5*(tk[i]+tk[i-1])) * cp2[1] + N03 (0.5*(tk[i]+tk[i-1])) * Yk[0] + N33 (0.5*(tk[i]+tk[i-1])) * Yk[N - 1]; midP = 0.5*(curP+prevP); Geom::Point diff; diff = curAppP-curP; curDist = dot(diff,diff); diff = midAppP-midP; midDist = dot(diff,diff); ndelta+=0.3333*(curDist+prevDist+midDist)/**lk[i]*/; if ( curDist > worstD ) { worstD = curDist; worstP = i; } else if ( fk[i] && 2*curDist > worstD ) { worstD = 2*curDist; worstP = i; } prevP = curP; prevAppP = curAppP; prevDist = curDist; } ndelta /= totLen; } else { #endif for (int i = 1; i < N - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; curAppP[0] = N13 (tk[i]) * cp1[0] + N23 (tk[i]) * cp2[0] + N03 (tk[i]) * Xk[0] + N33 (tk[i]) * Xk[N - 1]; curAppP[1] = N13 (tk[i]) * cp1[1] + N23 (tk[i]) * cp2[1] + N03 (tk[i]) * Yk[0] + N33 (tk[i]) * Yk[N - 1]; curP[0]=Xk[i]; curP[1]=Yk[i]; Geom::Point diff; diff=curAppP-curP; curDist=dot(diff,diff); ndelta+=curDist; if ( curDist > worstD ) { worstD=curDist; worstP=i; } else if ( fk[i] && 2*curDist > worstD ) { worstD=2*curDist; worstP=i; } prevP=curP; prevAppP=curAppP; prevDist=curDist; } #ifdef with_splotch_killer } #endif } g_free(tk); g_free(Qk); g_free(Xk); g_free(Yk); g_free(fk); g_free(lk); if (ndelta < delta + 0.00001) { return true; } else { // nothing better to do res.start = 3.0 * (cp1 - start); res.end = -3.0 * (cp2 - end); } return true; } else { // nothing better to do } g_free(tk); g_free(Qk); g_free(Xk); g_free(Yk); g_free(fk); g_free(lk); return false; }
Plane::Plane(const float3 &P,const float3 &n) { normal = n; d = -dot(normal, P); }
inline Vec3fa face_forward(const Vec3fa& dir, const Vec3fa& _Ng) { const Vec3fa Ng = _Ng; return dot(dir,Ng) < 0.0f ? Ng : neg(Ng); }
//----------------------------------------------------------------------------- // Frustum Triangle intersection using separating axis test. // note: the frustum and the triangle must be in the same space. // optimization needed bool FrustumTriangleIntersect(const Frustum& fr, const Triangle& tri) { bool allInside = true; for(int i = 0; i < 6; ++i) { const Plane& plane = fr[i]; float d1 = plane.Eval(tri.A); float d2 = plane.Eval(tri.B); float d3 = plane.Eval(tri.C); // if all outside a single plane, then there is // no intersection. if( d1 < 0 && d2 < 0 && d3 < 0) return false; allInside = allInside && ( d1 > 0 && d2 > 0 && d3 > 0); } // the tri is completely inside the frustum. if( allInside ) return true; // separating axis test. // Triangle: 3 edges 1 face normal. // Frustum: 6 edges, 5 face normal. // for total of 24 separating axis. // note this test can be optimized. // tri edges float3 triEdges[3]; triEdges[0] = tri.B - tri.A; triEdges[1] = tri.C - tri.A; triEdges[2] = tri.C - tri.B; // frustum edges float3 FrEdges[6]; FrEdges[0] = fr.Corner(Frustum::NearTopLeft) - fr.Corner(Frustum::NearTopRight); FrEdges[1] = fr.Corner(Frustum::NearBottomRight) - fr.Corner(Frustum::NearTopRight); FrEdges[2] = (fr.Corner(Frustum::FarBottonLeft) - fr.Corner(Frustum::NearBottonLeft)); FrEdges[3] = (fr.Corner(Frustum::FarBottomRight) - fr.Corner(Frustum::NearBottomRight)); FrEdges[4] = (fr.Corner(Frustum::FarTopRight) - fr.Corner(Frustum::NearTopRight)); FrEdges[5] = (fr.Corner(Frustum::FarTopLeft) - fr.Corner(Frustum::NearTopLeft)); int k = 0; float3 Axis[24]; Axis[k++] = fr.TopPlane().normal; Axis[k++] = fr.BottomPlane().normal; Axis[k++] = fr.LeftPlane().normal; Axis[k++] = fr.RightPlane().normal; Axis[k++] = fr.NearPlane().normal; Axis[k++] = normalize(cross(triEdges[0], triEdges[1])); for(int te = 0; te < 3; te++) { for(int fe = 0; fe < 6; fe++) { float3 axis = cross( triEdges[te], FrEdges[fe]); Axis[k++] = normalize(axis); } } for(int n = 0; n < 24; n++) { float3 axis = Axis[n]; if( lengthsquared(axis) < Epsilon) continue; float trid1 = dot(tri.A,axis); float trid2 = dot(tri.B,axis); float trid3 = dot(tri.C,axis); float trMin = std::min(trid1,trid2); trMin = std::min(trMin, trid3); float trMax = std::max(trid1,trid2); trMax = std::max(trMax,trid3); float frMin = dot(fr.Corner(0),axis); float frMax = frMin; for(int c = 1; c < 8; c++) { float fdist = dot(fr.Corner(c), axis); frMin = std::min(frMin,fdist); frMax = std::max(frMax,fdist); } if( (trMax < frMin) || (frMax < trMin)) { return false; } } // must be intersecting. return true; }
inline Vector2 project(const Vector2& other) const { return other * (dot(other) / other.dot(other)); }
double distance(const Vector3 &p, uint32_t tidx) const { const uint32_t idx0 = triangles[tidx]; const uint32_t idx1 = triangles[tidx + 1]; const uint32_t idx2 = triangles[tidx + 2]; const Vector3 v0 = vertices[idx0]; const Vector3 v1 = vertices[idx1]; const Vector3 v2 = vertices[idx2]; const Vector3 bv = v0; const Vector3 e0 = v1 - v0; const Vector3 e1 = v2 - v0; const Vector3 dv = bv - p; const double a = dot(e0, e0); const double b = dot(e0, e1); const double c = dot(e1, e1); const double d = dot(e0, dv); const double e = dot(e1, dv); const double f = dot(dv, dv); const double det = a*c - b*b; double s = b*e - c*d; double t = b*d - a*e; if (s + t <= det) { if (s < 0.0) { if (t < 0.0) { // region 4 if (d < 0.0) { t = 0.0; s = -d >= a ? 1.0 : -d / a; } else { s = 0.0; t = e >= 0.0 ? 0.0 : (-e >= c ? 1.0 : -e / c); } } else { // region 3 s = 0.0; t = e >= 0.0 ? 0.0 : (-e >= c ? 1.0 : -e / c); } } else if (t < 0.0) { // region 5 s = d >= 0.0 ? 0.0 : (-d >= a ? 1.0 : -d / a); t = 0.0; } else { // region 0 const double invDet = 1.0 / det; s *= invDet; t *= invDet; } } else { if (s < 0.0) { // region 2 const double tmp0 = b + d; const double tmp1 = c + e; if (tmp1 > tmp0) { const double numer = tmp1 - tmp0; const double denom = a - 2.0 * b + c; s = numer >= denom ? 1.0 : numer / denom; t = 1.0 - s; } else { s = 0.0; t = (tmp1 <= 0.0 ? 1.0 : (e >= 0.0 ? 0.0 : -e / c)); } } else if (t < 0.0) { // region 6 const double tmp0 = b + e; const double tmp1 = a + d; if (tmp1 > tmp0) { const double numer = tmp1 - tmp0; const double denom = a - 2.0 * b + c; t = numer >= denom ? 1.0 : numer / denom; s = 1.0 - t; } else { s = (tmp1 <= 0.0 ? 1.0 : (d >= 0.0 ? 0.0 : -d / a)); t = 0.0; } } else { // region 1 const double numer = c + e - b - d; if (numer <= 0) { s = 0.0; } else { const double denom = a - 2.0 * b + c; s = numer >= denom ? 1.0 : numer / denom; } t = 1.0 - s; } } return length(p - (v0 + Vector3(s) * e0 + Vector3(t) * e1)); }
PfxFloat pfxContactBoxCapsule( PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB, void *shapeA,const PfxTransform3 &transformA, void *shapeB,const PfxTransform3 &transformB, PfxFloat distanceThreshold) { PfxBox boxA = *((PfxBox*)shapeA); PfxCapsule capsuleB = *((PfxCapsule*)shapeB); PfxVector3 ident[3] = { PfxVector3(1.0,0.0,0.0), PfxVector3(0.0,1.0,0.0), PfxVector3(0.0,0.0,1.0), }; // get capsule position and direction in box's coordinate system PfxMatrix3 matrixA = transformA.getUpper3x3(); PfxMatrix3 matrixAinv = transpose(matrixA); PfxVector3 directionB = transformB.getUpper3x3().getCol0(); PfxVector3 translationB = transformB.getTranslation(); PfxVector3 capsDirection = matrixAinv * directionB; PfxVector3 absCapsDirection = absPerElem(capsDirection); PfxVector3 offsetAB = matrixAinv * (translationB - transformA.getTranslation()); // find separating axis with largest gap between projections BoxCapsSepAxisType axisType; PfxVector3 axisA; PfxFloat maxGap; int faceDimA = 0, edgeDimA = 0; // face axes // can compute all the gaps at once with VU0 PfxVector3 gapsA = absPerElem(offsetAB) - boxA.m_half - absCapsDirection * capsuleB.m_halfLen; AaxisTest( 0, X, true ); AaxisTest( 1, Y, false ); AaxisTest( 2, Z, false ); // cross product axes // compute gaps on all cross product axes using some VU0 math. suppose there's a tradeoff // between doing this with SIMD all at once or without SIMD in each cross product test, since // some test might exit early. PfxVector3 lsqrs, projOffset, projAhalf; PfxMatrix3 crossProdMat = crossMatrix(capsDirection) * PfxMatrix3::identity(); PfxMatrix3 crossProdMatT = crossMatrix(-capsDirection) * PfxMatrix3::identity(); lsqrs = mulPerElem( crossProdMatT.getCol0(), crossProdMatT.getCol0() ) + mulPerElem( crossProdMatT.getCol1(), crossProdMatT.getCol1() ) + mulPerElem( crossProdMatT.getCol2(), crossProdMatT.getCol2() ); projOffset = crossProdMatT * offsetAB; projAhalf = absPerElem(crossProdMatT) * boxA.m_half; PfxVector3 gapsAxB = absPerElem(projOffset) - projAhalf; CrossAxisTest( 0, X ); CrossAxisTest( 1, Y ); CrossAxisTest( 2, Z ); // make axis point from box center towards capsule center. if ( dot(axisA,offsetAB) < 0.0f ) axisA = -axisA; // find the face on box whose normal best matches the separating axis. will use the entire // face only in degenerate cases. // // to make things simpler later, change the coordinate system so that the face normal is the z // direction. if an edge cross product axis was chosen above, also align the box edge to the y // axis. this saves the later tests from having to know which face was chosen. changing the // coordinate system involves permuting vector elements, so construct a permutation matrix. // I believe this is a faster way to permute a bunch of vectors than using arrays. int dimA[3]; if ( axisType == CROSS_AXIS ) { PfxVector3 absAxisA = PfxVector3(absPerElem(axisA)); dimA[1] = edgeDimA; if ( edgeDimA == 0 ) { if ( absAxisA[1] > absAxisA[2] ) { dimA[0] = 2; dimA[2] = 1; } else { dimA[0] = 1; dimA[2] = 2; } } else if ( edgeDimA == 1 ) { if ( absAxisA[2] > absAxisA[0] ) { dimA[0] = 0; dimA[2] = 2; } else { dimA[0] = 2; dimA[2] = 0; } } else { if ( absAxisA[0] > absAxisA[1] ) { dimA[0] = 1; dimA[2] = 0; } else { dimA[0] = 0; dimA[2] = 1; } } } else { dimA[2] = faceDimA; dimA[0] = (faceDimA+1)%3; dimA[1] = (faceDimA+2)%3; } PfxMatrix3 aperm_col; aperm_col.setCol0(ident[dimA[0]]); aperm_col.setCol1(ident[dimA[1]]); aperm_col.setCol2(ident[dimA[2]]); PfxMatrix3 aperm_row = transpose(aperm_col); // permute vectors to be in face coordinate system. PfxVector3 offsetAB_perm = aperm_row * offsetAB; PfxVector3 halfA_perm = aperm_row * boxA.m_half; PfxVector3 signsA_perm = copySignPerElem(PfxVector3(1.0f), aperm_row * axisA); PfxVector3 scalesA_perm = mulPerElem( signsA_perm, halfA_perm ); PfxVector3 capsDirection_perm = aperm_row * capsDirection; PfxFloat signB = (-dot(capsDirection,axisA) > 0.0f)? 1.0f : -1.0f; PfxFloat scaleB = signB * capsuleB.m_halfLen; // compute the vector between the center of the box face and the capsule center offsetAB_perm.setZ( offsetAB_perm.getZ() - scalesA_perm.getZ() ); // if box and capsule overlap, this will separate them for finding points of penetration. if ( maxGap < 0.0f ) { offsetAB_perm -= aperm_row * axisA * maxGap * 1.01f; } // for each vertex/face or edge/edge pair of box face and line segment, find the closest // points. // // these points each have an associated feature (vertex, edge, or face). if each // point is in the external Voronoi region of the other's feature, they are the // closest points of the objects, and the algorithm can exit. // // the feature pairs are arranged so that in the general case, the first test will // succeed. degenerate cases (line segment parallel to face) may require up to all tests // in the worst case. // // if for some reason no case passes the Voronoi test, the features with the minimum // distance are returned. PfxVector3 closestPtsVec_perm; PfxPoint3 localPointA_perm; PfxFloat minDistSqr; PfxFloat segmentParamB; PfxBool done; localPointA_perm.setZ( scalesA_perm.getZ() ); scalesA_perm.setZ(0.0f); PfxVector3 hA_perm( halfA_perm ); int otherFaceDimA; if ( axisType == CROSS_AXIS ) { EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB, otherFaceDimA, hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm, scalesA_perm, true ); if ( !done ) { VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB, hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, false ); } } else { VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB, hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, true ); if ( !done ) { EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB, otherFaceDimA, hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm, scalesA_perm, false ); } } // compute normal PfxBool centerInside = ( signsA_perm.getZ() * closestPtsVec_perm.getZ() < 0.0f ); if ( centerInside || ( minDistSqr < lenSqrTol ) ) { normal = matrixA * axisA; } else { PfxVector3 closestPtsVec = aperm_col * closestPtsVec_perm; normal = matrixA * ( closestPtsVec * (1.0f/sqrtf( minDistSqr )) ); } // compute box point pointA = PfxPoint3( aperm_col * PfxVector3( localPointA_perm ) ); // compute capsule point pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( directionB * segmentParamB - normal * capsuleB.m_radius ) ); if ( centerInside ) { return (-sqrtf( minDistSqr ) - capsuleB.m_radius); } else { return (sqrtf( minDistSqr ) - capsuleB.m_radius); } }
double length(const Vector3 &a) { return std::sqrt(dot(a, a)); }
// 4D raw Simplex noise double raw_noise_4d( const double x, const double y, const double z, const double w ) { // The skewing and unskewing factors are hairy again for the 4D case double F4 = (sqrt(5.0)-1.0)/4.0; double G4 = (5.0-sqrt(5.0))/20.0; double n0, n1, n2, n3, n4; // Noise contributions from the five corners // Skew the (x,y,z,w) space to determine which cell of 24 simplices we're in double s = (x + y + z + w) * F4; // Factor for 4D skewing int i = fastfloor(x + s); int j = fastfloor(y + s); int k = fastfloor(z + s); int l = fastfloor(w + s); double t = (i + j + k + l) * G4; // Factor for 4D unskewing double X0 = i - t; // Unskew the cell origin back to (x,y,z,w) space double Y0 = j - t; double Z0 = k - t; double W0 = l - t; double x0 = x - X0; // The x,y,z,w distances from the cell origin double y0 = y - Y0; double z0 = z - Z0; double w0 = w - W0; // For the 4D case, the simplex is a 4D shape I won't even try to describe. // To find out which of the 24 possible simplices we're in, we need to // determine the magnitude ordering of x0, y0, z0 and w0. // The method below is a good way of finding the ordering of x,y,z,w and // then find the correct traversal order for the simplex we're in. // First, six pair-wise comparisons are performed between each possible pair // of the four coordinates, and the results are used to add up binary bits // for an integer index. int c1 = (x0 > y0) ? 32 : 0; int c2 = (x0 > z0) ? 16 : 0; int c3 = (y0 > z0) ? 8 : 0; int c4 = (x0 > w0) ? 4 : 0; int c5 = (y0 > w0) ? 2 : 0; int c6 = (z0 > w0) ? 1 : 0; int c = c1 + c2 + c3 + c4 + c5 + c6; int i1, j1, k1, l1; // The integer offsets for the second simplex corner int i2, j2, k2, l2; // The integer offsets for the third simplex corner int i3, j3, k3, l3; // The integer offsets for the fourth simplex corner // simplex[c] is a 4-vector with the numbers 0, 1, 2 and 3 in some order. // Many values of c will never occur, since e.g. x>y>z>w makes x<z, y<w and x<w // impossible. Only the 24 indices which have non-zero entries make any sense. // We use a thresholding to set the coordinates in turn from the largest magnitude. // The number 3 in the "simplex" array is at the position of the largest coordinate. i1 = simplex[c][0]>=3 ? 1 : 0; j1 = simplex[c][1]>=3 ? 1 : 0; k1 = simplex[c][2]>=3 ? 1 : 0; l1 = simplex[c][3]>=3 ? 1 : 0; // The number 2 in the "simplex" array is at the second largest coordinate. i2 = simplex[c][0]>=2 ? 1 : 0; j2 = simplex[c][1]>=2 ? 1 : 0; k2 = simplex[c][2]>=2 ? 1 : 0; l2 = simplex[c][3]>=2 ? 1 : 0; // The number 1 in the "simplex" array is at the second smallest coordinate. i3 = simplex[c][0]>=1 ? 1 : 0; j3 = simplex[c][1]>=1 ? 1 : 0; k3 = simplex[c][2]>=1 ? 1 : 0; l3 = simplex[c][3]>=1 ? 1 : 0; // The fifth corner has all coordinate offsets = 1, so no need to look that up. double x1 = x0 - i1 + G4; // Offsets for second corner in (x,y,z,w) coords double y1 = y0 - j1 + G4; double z1 = z0 - k1 + G4; double w1 = w0 - l1 + G4; double x2 = x0 - i2 + 2.0*G4; // Offsets for third corner in (x,y,z,w) coords double y2 = y0 - j2 + 2.0*G4; double z2 = z0 - k2 + 2.0*G4; double w2 = w0 - l2 + 2.0*G4; double x3 = x0 - i3 + 3.0*G4; // Offsets for fourth corner in (x,y,z,w) coords double y3 = y0 - j3 + 3.0*G4; double z3 = z0 - k3 + 3.0*G4; double w3 = w0 - l3 + 3.0*G4; double x4 = x0 - 1.0 + 4.0*G4; // Offsets for last corner in (x,y,z,w) coords double y4 = y0 - 1.0 + 4.0*G4; double z4 = z0 - 1.0 + 4.0*G4; double w4 = w0 - 1.0 + 4.0*G4; // Work out the hashed gradient indices of the five simplex corners int ii = i & 255; int jj = j & 255; int kk = k & 255; int ll = l & 255; int gi0 = perm[ii+perm[jj+perm[kk+perm[ll]]]] % 32; int gi1 = perm[ii+i1+perm[jj+j1+perm[kk+k1+perm[ll+l1]]]] % 32; int gi2 = perm[ii+i2+perm[jj+j2+perm[kk+k2+perm[ll+l2]]]] % 32; int gi3 = perm[ii+i3+perm[jj+j3+perm[kk+k3+perm[ll+l3]]]] % 32; int gi4 = perm[ii+1+perm[jj+1+perm[kk+1+perm[ll+1]]]] % 32; // Calculate the contribution from the five corners double t0 = 0.6 - x0*x0 - y0*y0 - z0*z0 - w0*w0; if(t0<0) n0 = 0.0; else { t0 *= t0; n0 = t0 * t0 * dot(grad4[gi0], x0, y0, z0, w0); } double t1 = 0.6 - x1*x1 - y1*y1 - z1*z1 - w1*w1; if(t1<0) n1 = 0.0; else { t1 *= t1; n1 = t1 * t1 * dot(grad4[gi1], x1, y1, z1, w1); } double t2 = 0.6 - x2*x2 - y2*y2 - z2*z2 - w2*w2; if(t2<0) n2 = 0.0; else { t2 *= t2; n2 = t2 * t2 * dot(grad4[gi2], x2, y2, z2, w2); } double t3 = 0.6 - x3*x3 - y3*y3 - z3*z3 - w3*w3; if(t3<0) n3 = 0.0; else { t3 *= t3; n3 = t3 * t3 * dot(grad4[gi3], x3, y3, z3, w3); } double t4 = 0.6 - x4*x4 - y4*y4 - z4*z4 - w4*w4; if(t4<0) n4 = 0.0; else { t4 *= t4; n4 = t4 * t4 * dot(grad4[gi4], x4, y4, z4, w4); } // Sum up and scale the result to cover the range [-1,1] return 27.0 * (n0 + n1 + n2 + n3 + n4); }
Vector3 normalize(const Vector3 &a) { const double lrcp = 1.0 / std::sqrt(dot(a, a)); return Vector3(a.x * lrcp, a.y * lrcp, a.z * lrcp); }
/* Determines if the satellite is in sunlight or shadow. * Taken from Montenbruck and Gill p. 80-83 * @param r ECI position vector of spacecraft [m]. * @param r_Sun Sun position vector (geocentric) [m]. * @param r_Moon Moon position vector (geocentric) [m]. * @return 0.0 if in shadow, 1.0 if in sunlight, 0 to 1.0 if in partial shadow */ double SolarRadiationPressure::getShadowFunction(Vector<double> r, Vector<double> r_Sun, Vector<double> r_Moon, SolarRadiationPressure::ShadowModel sm) { // shadow function double v = 0.0; // Mean Radious of Sun, Moon and Earth const double R_sun = ASConstant::R_Sun; const double R_moon = ASConstant::R_Moon; #pragma unused(R_moon) const double R_earth = ASConstant::R_Earth; Vector<double> e_Sun = r_Sun/norm(r_Sun); // Sun direction unit vector double r_dot_sun = dot(r,e_Sun); if(r_dot_sun>0) { // Sunny side of central body is always fully lit and return v= 1.0; return v; } if(sm == SM_CYLINDRICAL) { // Taken fram Jisheng Li P111, and checked with GMAT and Bernese5 SHADOW.f v = ((r_dot_sun>0 || norm(r-r_dot_sun*e_Sun)>R_earth) ? 1.0 : 0.0); return v; } else if(sm == SM_CONICAL) { /* // Taken from Montenbruck and Gill p. 80-83 double s0, s2; // Montenbruck and Gill, eq. 3.79 s0 = -dot(r,e_Sun); //-state[0]*unitsun[0] - state[1]*unitsun[1] - state[2]*unitsun[2]; s2 = dot(r,r); //state[0]*state[0] + state[1]*state[1] + state[2]*state[2]; // Montenbruck and Gill, eq. 3.80 double lsc = sqrt(s2 - s0*s0); // Montenbruck and Gill, eq. 3.81 double sinf1 = (R_sun + R_earth) / norm(r_Sun); double sinf2 = (R_earth - R_earth) / norm(r_Sun); // Appropriate l1 and l2 temporarily double t1 = sinf1 * sinf1; double t2 = sinf2 * sinf2; double tanf1 = sqrt(t1 / (1.0 - t1)); double tanf2 = sqrt(t2 / (1.0 - t2)); // Montenbruck and Gill, eq. 3.82 double c1 = s0 + R_earth / sinf1; double c2 = R_earth / sinf2 - s0; // Different sign from M&G // Montenbruck and Gill, eq. 3.83 double l1 = c1 * tanf1; double l2 = c2 * tanf2; if (lsc > l1) // Outside of the penumbral cone { v = 1.0; return v; } else { //lit = false; if (lsc < fabs(l2)) { // Inside umbral cone if (c2 >= 0.0) { // no annular ring percentSun = 0.0; dark = true; } else { // annular eclipse pcbrad = asin(R_earth / sqrt(s2)); percentSun = (psunrad*psunrad - pcbrad*pcbrad) / (psunrad*psunrad); dark = false; } return; } // In penumbra pcbrad = asin(R_earth / sqrt(s2)); percentSun = ShadowFunction(state); lit = false; dark = false; }*/ ////////////////////////////////////////////////////////////////////////// double r_sun_mag = norm(r_Sun); #pragma unused(r_sun_mag) double r_mag = norm(r); Vector<double> d = r_Sun-r; // vector from sc to sun double dmag = norm(d); double a = std::asin(R_sun/dmag); // eq. 3.85 double b = std::asin(R_earth/r_mag); // eq. 3.86 double c = std::acos(-1.0*dot(r,d)/(r_mag*dmag)); // eq. 3.87 if((a+b)<=c) // in Sun light { v = 1.0; } else if(c < (b-a)) // in Umbra { v = 0.0; } else // in Penumbra { double x = (c*c+a*a-b*b)/(2*c); // eq. 3.93 double y = std::sqrt(a*a-x*x); double A = a*a*std::acos(x/a)+b*b*std::acos((c-x)/b)-c*y; // eq. 3.92 v = 1.0 - A/(ASConstant::PI*a*a); // eq. 3.94 } return v; } else { // unexpected value Exception e("Unexpect ShadowModel in getShadowFunction()"); GPSTK_THROW(e); } return v; } // End of method 'SolarRadiationPressure::getShadowFunction()'
Spectrum Li(const RayDifferential &ray, RadianceQueryRecord &rRec) const { Spectrum LiSurf(0.0f), LiMedium(0.0f), transmittance(1.0f); Intersection &its = rRec.its; const Scene *scene = rRec.scene; bool cacheQuery = (rRec.extra & RadianceQueryRecord::ECacheQuery); bool adaptiveQuery = (rRec.extra & RadianceQueryRecord::EAdaptiveQuery); /* Perform the first ray intersection (or ignore if the intersection has already been provided). */ rRec.rayIntersect(ray); if (rRec.medium) { Ray mediumRaySegment(ray, 0, its.t); transmittance = rRec.medium->evalTransmittance(mediumRaySegment); mediumRaySegment.mint = ray.mint; if (rRec.type & RadianceQueryRecord::EVolumeRadiance && (rRec.depth < m_maxDepth || m_maxDepth < 0) && m_bre.get() != NULL) LiMedium = m_bre->query(mediumRaySegment, rRec.medium); } if (!its.isValid()) { /* If no intersection could be found, possibly return attenuated radiance from a background luminaire */ if ((rRec.type & RadianceQueryRecord::EEmittedRadiance) && !m_hideEmitters) LiSurf = scene->evalEnvironment(ray); return LiSurf * transmittance + LiMedium; } /* Possibly include emitted radiance if requested */ if (its.isEmitter() && (rRec.type & RadianceQueryRecord::EEmittedRadiance) && !m_hideEmitters) LiSurf += its.Le(-ray.d); /* Include radiance from a subsurface scattering model if requested */ if (its.hasSubsurface() && (rRec.type & RadianceQueryRecord::ESubsurfaceRadiance)) LiSurf += its.LoSub(scene, rRec.sampler, -ray.d, rRec.depth); const BSDF *bsdf = its.getBSDF(ray); if (rRec.depth >= m_maxDepth && m_maxDepth > 0) return LiSurf * transmittance + LiMedium; unsigned int bsdfType = bsdf->getType() & BSDF::EAll; /* Irradiance cache query -> treat as diffuse */ bool isDiffuse = (bsdfType == BSDF::EDiffuseReflection) || cacheQuery; bool hasSpecular = bsdfType & BSDF::EDelta; /* Exhaustively recurse into all specular lobes? */ bool exhaustiveSpecular = rRec.depth < m_maxSpecularDepth && !cacheQuery; if (isDiffuse && (dot(its.shFrame.n, ray.d) < 0 || (bsdf->getType() & BSDF::EBackSide))) { /* 1. Diffuse indirect */ int maxDepth = m_maxDepth == -1 ? INT_MAX : (m_maxDepth-rRec.depth); if (rRec.type & RadianceQueryRecord::EIndirectSurfaceRadiance && m_globalPhotonMap.get()) LiSurf += m_globalPhotonMap->estimateIrradiance(its.p, its.shFrame.n, m_globalLookupRadius, maxDepth, m_globalLookupSize) * bsdf->getDiffuseReflectance(its) * INV_PI; if (rRec.type & RadianceQueryRecord::ECausticRadiance && m_causticPhotonMap.get()) LiSurf += m_causticPhotonMap->estimateIrradiance(its.p, its.shFrame.n, m_causticLookupRadius, maxDepth, m_causticLookupSize) * bsdf->getDiffuseReflectance(its) * INV_PI; } if (hasSpecular && exhaustiveSpecular && (rRec.type & RadianceQueryRecord::EIndirectSurfaceRadiance)) { /* 1. Specular indirect */ int compCount = bsdf->getComponentCount(); RadianceQueryRecord rRec2; for (int i=0; i<compCount; i++) { unsigned int type = bsdf->getType(i); if (!(type & BSDF::EDelta)) continue; /* Sample the BSDF and recurse */ BSDFSamplingRecord bRec(its, rRec.sampler, ERadiance); bRec.component = i; Spectrum bsdfVal = bsdf->sample(bRec, Point2(0.5f)); if (bsdfVal.isZero()) continue; rRec2.recursiveQuery(rRec, RadianceQueryRecord::ERadiance); RayDifferential bsdfRay(its.p, its.toWorld(bRec.wo), ray.time); if (its.isMediumTransition()) rRec2.medium = its.getTargetMedium(bsdfRay.d); LiSurf += bsdfVal * m_parentIntegrator->Li(bsdfRay, rRec2); } } /* Estimate the direct illumination if this is requested */ int numEmitterSamples = m_directSamples, numBSDFSamples; Float weightLum, weightBSDF; Point2 *sampleArray; Point2 sample; if (rRec.depth > 1 || cacheQuery || adaptiveQuery) { /* This integrator is used recursively by another integrator. Be less accurate as this sample will not directly be observed. */ numBSDFSamples = numEmitterSamples = 1; weightLum = weightBSDF = 1.0f; } else { if (isDiffuse) { numBSDFSamples = m_directSamples; weightBSDF = weightLum = m_invEmitterSamples; } else { numBSDFSamples = m_glossySamples; weightLum = m_invEmitterSamples; weightBSDF = m_invGlossySamples; } } if ((bsdfType & BSDF::ESmooth) && (rRec.type & RadianceQueryRecord::EDirectSurfaceRadiance)) { DirectSamplingRecord dRec(its); if (numEmitterSamples > 1) { sampleArray = rRec.sampler->next2DArray(m_directSamples); } else { sample = rRec.nextSample2D(); sampleArray = &sample; } for (int i=0; i<numEmitterSamples; ++i) { int interactions = m_maxDepth - rRec.depth - 1; Spectrum value = scene->sampleAttenuatedEmitterDirect( dRec, its, rRec.medium, interactions, sampleArray[i], rRec.sampler); /* Estimate the direct illumination if this is requested */ if (!value.isZero()) { const Emitter *emitter = static_cast<const Emitter *>(dRec.object); /* Allocate a record for querying the BSDF */ BSDFSamplingRecord bRec(its, its.toLocal(dRec.d)); /* Evaluate BSDF * cos(theta) */ const Spectrum bsdfVal = bsdf->eval(bRec); if (!bsdfVal.isZero()) { /* Calculate prob. of having sampled that direction using BSDF sampling */ if (!hasSpecular || exhaustiveSpecular) bRec.typeMask = BSDF::ESmooth; Float bsdfPdf = (emitter->isOnSurface() && dRec.measure == ESolidAngle && interactions == 0) ? bsdf->pdf(bRec) : (Float) 0.0f; /* Weight using the power heuristic */ const Float weight = miWeight(dRec.pdf * numEmitterSamples, bsdfPdf * numBSDFSamples) * weightLum; LiSurf += value * bsdfVal * weight; } } } } /* ==================================================================== */ /* BSDF sampling */ /* ==================================================================== */ /* Sample direct compontent via BSDF sampling if this is generally requested AND the BSDF is smooth, or there is a delta component that was not handled by the exhaustive sampling loop above */ bool bsdfSampleDirect = (rRec.type & RadianceQueryRecord::EDirectSurfaceRadiance) && ((bsdfType & BSDF::ESmooth) || (hasSpecular && !exhaustiveSpecular)); /* Sample indirect component via BSDF sampling if this is generally requested AND the BSDF is non-diffuse (diffuse is handled by the global photon map) or there is a delta component that was not handled by the exhaustive sampling loop above. */ bool bsdfSampleIndirect = (rRec.type & RadianceQueryRecord::EIndirectSurfaceRadiance) && !isDiffuse && ((bsdfType & BSDF::ESmooth) || (hasSpecular && !exhaustiveSpecular)); if (bsdfSampleDirect || bsdfSampleIndirect) { if (numBSDFSamples > 1) { sampleArray = rRec.sampler->next2DArray( std::max(m_directSamples, m_glossySamples)); } else { sample = rRec.nextSample2D(); sampleArray = &sample; } RadianceQueryRecord rRec2; Intersection &bsdfIts = rRec2.its; DirectSamplingRecord dRec(its); for (int i=0; i<numBSDFSamples; ++i) { /* Sample BSDF * cos(theta) */ BSDFSamplingRecord bRec(its, rRec.sampler, ERadiance); if (!hasSpecular || exhaustiveSpecular) bRec.typeMask = BSDF::ESmooth; Float bsdfPdf; Spectrum bsdfVal = bsdf->sample(bRec, bsdfPdf, sampleArray[i]); if (bsdfVal.isZero()) continue; /* Trace a ray in this direction */ RayDifferential bsdfRay(its.p, its.toWorld(bRec.wo), ray.time); Spectrum value; bool hitEmitter = false; if (scene->rayIntersect(bsdfRay, bsdfIts)) { /* Intersected something - check if it was a luminaire */ if (bsdfIts.isEmitter() && bsdfSampleDirect) { value = bsdfIts.Le(-bsdfRay.d); dRec.setQuery(bsdfRay, bsdfIts); hitEmitter = true; } } else if (bsdfSampleDirect) { /* Intersected nothing -- perhaps there is an environment map? */ const Emitter *env = scene->getEnvironmentEmitter(); if (env) { value = env->evalEnvironment(bsdfRay); if (env->fillDirectSamplingRecord(dRec, bsdfRay)) hitEmitter = true; } } if (hitEmitter) { const Float emitterPdf = scene->pdfEmitterDirect(dRec); Spectrum transmittance = rRec2.medium ? rRec2.medium->evalTransmittance(Ray(bsdfRay, 0, bsdfIts.t)) : Spectrum(1.0f); const Float weight = miWeight(bsdfPdf * numBSDFSamples, emitterPdf * numEmitterSamples) * weightBSDF; LiSurf += value * bsdfVal * weight * transmittance; } /* Recurse */ if (bsdfSampleIndirect) { rRec2.recursiveQuery(rRec, RadianceQueryRecord::ERadianceNoEmission); rRec2.type ^= RadianceQueryRecord::EIntersection; if (its.isMediumTransition()) rRec2.medium = its.getTargetMedium(bsdfRay.d); LiSurf += bsdfVal * m_parentIntegrator->Li(bsdfRay, rRec2) * weightBSDF; } } } return LiSurf * transmittance + LiMedium; }
float operator*(const Vec4 & lhs, const Vec4 & rhs) { return dot(lhs, rhs); }
// fit a polyline to a bezier patch, return true is treshhold not exceeded (ie: you can continue) // version that uses tables from the previous iteration, to minimize amount of work done bool Path::AttemptSimplify (fitting_tables &data,double treshhold, PathDescrCubicTo & res,int &worstP) { Geom::Point start,end; // pour une coordonnee Geom::Point cp1, cp2; worstP = 1; if (pts.size() == 2) { return true; } start[0] = data.Xk[0]; start[1] = data.Yk[0]; cp1[0] = data.Xk[1]; cp1[1] = data.Yk[1]; end[0] = data.Xk[data.nbPt - 1]; end[1] = data.Yk[data.nbPt - 1]; cp2 = cp1; if (pts.size() == 3) { // start -> cp1 -> end res.start = cp1 - start; res.end = end - cp1; worstP = 1; return true; } if ( FitCubic(start, res, data.Xk, data.Yk, data.Qk, data.tk, data.nbPt) ) { cp1 = start + res.start / 3; cp2 = end - res.end / 3; } else { // aie, non-inversible double worstD = 0; worstP = -1; for (int i = 1; i < data.nbPt; i++) { Geom::Point nPt; nPt[Geom::X] = data.Xk[i]; nPt[Geom::Y] = data.Yk[i]; double nle = DistanceToCubic(start, res, nPt); if ( data.fk[i] ) { // forced points are favored for splitting the recursion; we do this by increasing their distance if ( worstP < 0 || 2 * nle > worstD ) { worstP = i; worstD = 2 * nle; } } else { if ( worstP < 0 || nle > worstD ) { worstP = i; worstD = nle; } } } return false; } // calcul du delta= pondere par les longueurs des segments double delta = 0; { double worstD = 0; worstP = -1; Geom::Point prevAppP; Geom::Point prevP; double prevDist; prevP[Geom::X] = data.Xk[0]; prevP[Geom::Y] = data.Yk[0]; prevAppP = prevP; // le premier seulement prevDist = 0; #ifdef with_splotch_killer if ( data.nbPt <= 20 ) { for (int i = 1; i < data.nbPt - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; Geom::Point midAppP; Geom::Point midP; double midDist; curAppP[Geom::X] = N13(data.tk[i]) * cp1[Geom::X] + N23(data.tk[i]) * cp2[Geom::X] + N03(data.tk[i]) * data.Xk[0] + N33(data.tk[i]) * data.Xk[data.nbPt - 1]; curAppP[Geom::Y] = N13(data.tk[i]) * cp1[Geom::Y] + N23(data.tk[i]) * cp2[Geom::Y] + N03(data.tk[i]) * data.Yk[0] + N33(data.tk[i]) * data.Yk[data.nbPt - 1]; curP[Geom::X] = data.Xk[i]; curP[Geom::Y] = data.Yk[i]; double mtk = 0.5 * (data.tk[i] + data.tk[i - 1]); midAppP[Geom::X] = N13(mtk) * cp1[Geom::X] + N23(mtk) * cp2[Geom::X] + N03(mtk) * data.Xk[0] + N33(mtk) * data.Xk[data.nbPt - 1]; midAppP[Geom::Y] = N13(mtk) * cp1[Geom::Y] + N23(mtk) * cp2[Geom::Y] + N03(mtk) * data.Yk[0] + N33(mtk) * data.Yk[data.nbPt - 1]; midP = 0.5 * (curP + prevP); Geom::Point diff = curAppP - curP; curDist = dot(diff, diff); diff = midAppP - midP; midDist = dot(diff, diff); delta += 0.3333 * (curDist + prevDist + midDist) * data.lk[i]; if ( curDist > worstD ) { worstD = curDist; worstP = i; } else if ( data.fk[i] && 2 * curDist > worstD ) { worstD = 2*curDist; worstP = i; } prevP = curP; prevAppP = curAppP; prevDist = curDist; } delta /= data.totLen; } else { #endif for (int i = 1; i < data.nbPt - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; curAppP[Geom::X] = N13(data.tk[i]) * cp1[Geom::X] + N23(data.tk[i]) * cp2[Geom::X] + N03(data.tk[i]) * data.Xk[0] + N33(data.tk[i]) * data.Xk[data.nbPt - 1]; curAppP[Geom::Y] = N13(data.tk[i]) * cp1[Geom::Y] + N23(data.tk[i]) * cp2[Geom::Y] + N03(data.tk[i]) * data.Yk[0] + N33(data.tk[i]) * data.Yk[data.nbPt - 1]; curP[Geom::X] = data.Xk[i]; curP[Geom::Y] = data.Yk[i]; Geom::Point diff = curAppP-curP; curDist = dot(diff, diff); delta += curDist; if ( curDist > worstD ) { worstD = curDist; worstP = i; } else if ( data.fk[i] && 2 * curDist > worstD ) { worstD = 2*curDist; worstP = i; } prevP = curP; prevAppP = curAppP; prevDist = curDist; } #ifdef with_splotch_killer } #endif } if (delta < treshhold * treshhold) { // premier jet // Refine a little. for (int i = 1; i < data.nbPt - 1; i++) { Geom::Point pt(data.Xk[i], data.Yk[i]); data.tk[i] = RaffineTk(pt, start, cp1, cp2, end, data.tk[i]); if (data.tk[i] < data.tk[i - 1]) { // Force tk to be monotonic non-decreasing. data.tk[i] = data.tk[i - 1]; } } if ( FitCubic(start, res, data.Xk, data.Yk, data.Qk, data.tk, data.nbPt) == false) { // ca devrait jamais arriver, mais bon res.start = 3.0 * (cp1 - start); res.end = 3.0 * (end - cp2 ); return true; } double ndelta = 0; { double worstD = 0; worstP = -1; Geom::Point prevAppP; Geom::Point prevP(data.Xk[0], data.Yk[0]); double prevDist = 0; prevAppP = prevP; // le premier seulement #ifdef with_splotch_killer if ( data.nbPt <= 20 ) { for (int i = 1; i < data.nbPt - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; Geom::Point midAppP; Geom::Point midP; double midDist; curAppP[Geom::X] = N13(data.tk[i]) * cp1[Geom::X] + N23(data.tk[i]) * cp2[Geom::X] + N03(data.tk[i]) * data.Xk[0] + N33(data.tk[i]) * data.Xk[data.nbPt - 1]; curAppP[Geom::Y] = N13(data.tk[i]) * cp1[Geom::Y] + N23(data.tk[i]) * cp2[Geom::Y] + N03(data.tk[i]) * data.Yk[0] + N33(data.tk[i]) * data.Yk[data.nbPt - 1]; curP[Geom::X] = data.Xk[i]; curP[Geom::Y] = data.Yk[i]; double mtk = 0.5 * (data.tk[i] + data.tk[i - 1]); midAppP[Geom::X] = N13(mtk) * cp1[Geom::X] + N23(mtk) * cp2[Geom::X] + N03(mtk) * data.Xk[0] + N33(mtk) * data.Xk[data.nbPt - 1]; midAppP[Geom::Y] = N13(mtk) * cp1[Geom::Y] + N23(mtk) * cp2[Geom::Y] + N03(mtk) * data.Yk[0] + N33(mtk) * data.Yk[data.nbPt - 1]; midP = 0.5 * (curP + prevP); Geom::Point diff = curAppP - curP; curDist = dot(diff, diff); diff = midAppP - midP; midDist = dot(diff, diff); ndelta += 0.3333 * (curDist + prevDist + midDist) * data.lk[i]; if ( curDist > worstD ) { worstD = curDist; worstP = i; } else if ( data.fk[i] && 2 * curDist > worstD ) { worstD = 2*curDist; worstP = i; } prevP = curP; prevAppP = curAppP; prevDist = curDist; } ndelta /= data.totLen; } else { #endif for (int i = 1; i < data.nbPt - 1; i++) { Geom::Point curAppP; Geom::Point curP; double curDist; curAppP[Geom::X] = N13(data.tk[i]) * cp1[Geom::X] + N23(data.tk[i]) * cp2[Geom::X] + N03(data.tk[i]) * data.Xk[0] + N33(data.tk[i]) * data.Xk[data.nbPt - 1]; curAppP[Geom::Y] = N13(data.tk[i]) * cp1[Geom::Y] + N23(data.tk[i]) * cp2[1] + N03(data.tk[i]) * data.Yk[0] + N33(data.tk[i]) * data.Yk[data.nbPt - 1]; curP[Geom::X] = data.Xk[i]; curP[Geom::Y] = data.Yk[i]; Geom::Point diff = curAppP - curP; curDist = dot(diff, diff); ndelta += curDist; if ( curDist > worstD ) { worstD = curDist; worstP = i; } else if ( data.fk[i] && 2 * curDist > worstD ) { worstD = 2 * curDist; worstP = i; } prevP = curP; prevAppP = curAppP; prevDist = curDist; } #ifdef with_splotch_killer } #endif } if (ndelta < delta + 0.00001) { return true; } else { // nothing better to do res.start = 3.0 * (cp1 - start); res.end = 3.0 * (end - cp2 ); } return true; } return false; }
void cartes(double *x, double *v, ELEMS elem_ptr,double mu) { double a,e,m,cosi,sini,cos_lasc,sin_lasc,cos_aper,sin_aper; double es,ec,w,wp,wpp,wppp,ecc,dx,lo,up,next;int iter; double sin_ecc,cos_ecc,l1,m1,n1,l2,m2,n2; double xi,eta,vel_scl; a = elem_ptr.a; e = elem_ptr.e; m = elem_ptr.mean_an; cosi = cos(elem_ptr.i); sini = sin(elem_ptr.i); cos_lasc = cos(elem_ptr.lasc); sin_lasc = sin(elem_ptr.lasc); cos_aper = cos(elem_ptr.aper); sin_aper = sin(elem_ptr.aper); /* * Reduce mean anomoly to [0, 2*PI) */ m -= ((int)(m/(2*M_PI)))*2*M_PI; /* * Solve kepler's equation. */ if (sin(m)>0) ecc = m+0.85*e; else ecc = m-0.85*e; lo = -2*M_PI; up = 2*M_PI; for(iter=1;iter<=32;iter++) { es = e*sin(ecc); ec = e*cos(ecc); w = ecc-es-m; wp = 1-ec; wpp = es; wppp = ec; if (w>0) up = ecc; else lo = ecc; dx = -w/wp; dx = -w/(wp+dx*wpp/2); dx = -w/(wp+dx*wpp/2+dx*dx*wppp/6); next = ecc+dx; if (ecc==next) break; if ((next>lo) && (next<up)) ecc= next; else ecc= (lo+up)/2; if((ecc==lo) || (ecc==up)) break; if (iter>30) printf("%4d %23.20f %e\n",iter,ecc,up-lo); } if(iter>32) { fprintf(stderr,"ERROR: Kepler solultion failed.\n"); exit(1); } cos_ecc = cos(ecc); sin_ecc = sin(ecc); l1 = cos_lasc*cos_aper-sin_lasc*sin_aper*cosi; m1 = sin_lasc*cos_aper+cos_lasc*sin_aper*cosi; n1 = sin_aper*sini; l2 = -cos_lasc*sin_aper-sin_lasc*cos_aper*cosi; m2 = -sin_lasc*sin_aper+cos_lasc*cos_aper*cosi; n2 = cos_aper*sini; xi = a*(cos_ecc-e); eta = a*sqrt(1-e*e)*sin_ecc; x[0] = l1*xi+l2*eta; x[1] = m1*xi+m2*eta; x[2] = n1*xi+n2*eta; vel_scl = sqrt((mu*a)/dot(x,x)); xi = -vel_scl*sin_ecc; eta = vel_scl*sqrt(1-e*e)*cos_ecc; v[0] = l1*xi+l2*eta; v[1] = m1*xi+m2*eta; v[2] = n1*xi+n2*eta; }
int main(void) { // accessors Vector<3> v; v[0] = 1.f; v[1] = 1.f; v[2] = 1.f; std::cout << "testing the accessors..." << std::endl; assert(fabs(v.x() - 1.f) < 0.0001); assert(fabs(v.y() - 1.f) < 0.0001); assert(fabs(v.z() - 1.f) < 0.0001); std::cout << "accessors working!" << std::endl; Vector<3> v1, v2, vr, vc; v1[0] = v1[1] = v1[2] = v2[0] = v2[1] = v2[2] = 1.f; // operator== assert(v1 == v2); // operator!= vr = v1 + v2; assert(v1 != vr); // operator+ vr = v1 + 1.f; assert(vr == Vector<3>(2.f, 2.f, 2.f)); vr = v1 + v2; assert(vr == Vector<3>(2.f, 2.f, 2.f)); // operator- vr = v1 - 1.f; assert(vr == Vector<3>(0.f, 0.f, 0.f)); vr = v1 - v2; assert(vr == Vector<3>(0.f, 0.f, 0.f)); // operator* vr = v1 * 1.f; assert(vr == Vector<3>(1.f, 1.f, 1.f)); vr = v1 * v2; assert(vr == Vector<3>(1.f, 1.f, 1.f)); // operator/ vr = v1 / 1.f; assert(vr == Vector<3>(1.f, 1.f, 1.f)); vr = v1 / v2; assert(vr == Vector<3>(1.f, 1.f, 1.f)); // dot float d = dot(v1, v2); assert(fabs(d - 3.f) < 0.0001); // cross vr = cross(Vector<3>(0.f, 1.f, 0.f), Vector<3>(1.f, 0.f, 0.f)); assert(vr == Vector<3>(0.f, 0.f, -1.f)); // length float l = vr.length(); assert(fabs(l - 1.f) < 0.0001); // normal Vector<3> n = vr.normal(); vr.normalize(); assert(n == vr); assert(n == Vector<3>(0.f, 0.f, -1.f)); // normalize Vector<3> three(3.f, 0.f, 0.f); three.normalize(); assert(three == Vector<3>(1.f, 0.f, 0.f)); // distance float dist = distance(Vector<3>(0.f, 0.f, 0.f), Vector<3>(1.f, 0.f, 0.f)); assert(fabs(dist - 1.f) < 0.0001); // int assert((Vector<3, int>(0, 0, 0) == Vector<3, int>(0, 0, 0))); assert((Vector<3, int>(0, 0, 0) != Vector<3, int>(1, 0, -1))); assert((Vector<3, int>(1, 0, 0) < Vector<3, int>(2, 0, 0))); assert((Vector<3, int>(3, 0, 0) < Vector<3, int>(2, 0, 1))); assert((Vector<3, int>(3, 9, 0) < Vector<3, int>(2, 0, 1))); assert((Vector<3, int>(-1, 0, -1) < Vector<3, int>(-1, -1, 0))); assert((Vector<3, int>(-1, 0, -1) < Vector<3, int>(-1, -1, 1))); assert((Vector<3, int>(-1, 0, -1) > Vector<3, int>(-1, -1, -1))); assert((Vector<3, int>(-1, -1, -1) < Vector<3, int>(0, -1, -1))); Vector<3, int> copy = Vector<3, int>(-1, 0, 1); assert((copy == Vector<3, int>(-1, 0, 1))); // map std::map<Vector<3, int>, int> vMap; // vMap[Vector<3, int>(-1, -1, -1)] = 1; // vMap[Vector<3, int>(-1, -1, 0)] = 1; vMap[Vector<3, int>(-1, -1, 1)] = 1; vMap[Vector<3, int>(-1, 0, -1)] = 1; std::map<Vector<3, int>, int>::iterator itr = vMap.find(Vector<3, int>(-1, 0, -1)); std::cout << itr->first << std::endl; assert((itr->first == Vector<3, int>(-1, 0, -1))); std::cout << "all working!" << std::endl; return 0; }