Esempio n. 1
0
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);
  
  
  
}
Esempio n. 2
0
	/// 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;
	}
Esempio n. 3
0
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);
}
Esempio n. 4
0
// 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;
}
Esempio n. 6
0
/* 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;
}
Esempio n. 7
0
/* >>> 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;
}
Esempio n. 8
0
 //================== Plane ============
 Plane::Plane(const float3 &A,const float3 &B,const float3 &C)
 {
     normal = normalize( cross((B-A),(C-A)) );
     d = -dot(normal, A);
 }
Esempio n. 9
0
    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;
        }
    }
Esempio n. 10
0
 inline Vector3D reflect(const Vector3D &v, const Vector3D &n) {
   return v - 2*dot(n, v)*n;
 }
Esempio n. 11
0
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;
}
Esempio n. 12
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;
}
Esempio n. 13
0
File: main.c Progetto: 2bt/raytracer
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);
}
Esempio n. 14
0
	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;
	}
Esempio n. 15
0
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;
}
Esempio n. 16
0
 Plane::Plane(const float3 &P,const float3 &n)
 {
     normal = n;
     d = -dot(normal, P);
 }
Esempio n. 17
0
inline Vec3fa face_forward(const Vec3fa& dir, const Vec3fa& _Ng) {
  const Vec3fa Ng = _Ng;
  return dot(dir,Ng) < 0.0f ? Ng : neg(Ng);
}
Esempio n. 18
0
    //-----------------------------------------------------------------------------
    // 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)); }
Esempio n. 20
0
	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);
	}
}
Esempio n. 22
0
double length(const Vector3 &a)
{
	return std::sqrt(dot(a, a));
}
Esempio n. 23
0
// 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);
}
Esempio n. 24
0
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);
}
Esempio n. 25
0
      /* 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()'
Esempio n. 26
0
	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;
	}
Esempio n. 27
0
float operator*(const Vec4 & lhs, const Vec4 & rhs) {
	return dot(lhs, rhs);
}
Esempio n. 28
0
// 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;
}
Esempio n. 29
0
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;
}
Esempio n. 30
0
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;
}