//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
Line3D::GetDistanceTo(
    const Sphere &sphere,
    Scalar *penetration
) const
{
    Check_Object(this);
    Check_Object(&sphere);
    Check_Pointer(penetration);

    //
    //-------------------------------------------------------------------
    // Determine if ray intersects bounding sphere of object.  If sphere
    // is (X-C)*(X-C) = R^2 and ray is X = t*D+L for t >= 0, then
    // intersection is obtained by plugging X into sphere equation to
    // get quadratic:  (D*D)t^2 + 2*(D*(L-C))t + (L-C)*(L-C) = 0
    // Define a = D*D = 1.0f, b = 2*(D*(L-C)), and c = (L-C)*(L-C).
    //-------------------------------------------------------------------
    //
    Vector3D diff;
    diff.Subtract(origin, sphere.center);
    Scalar b = (direction*diff) * 2.0f;
    Scalar c = (diff*diff) - sphere.radius*sphere.radius;

    //
    //-------------------------------------------------------------------------
    // If penetration is negative, we couldn't hit the sphere at all.  If it is
    // really small, it touches at only one place
    //-------------------------------------------------------------------------
    //
    *penetration = b*b - 4.0f*c;
    if (*penetration < -SMALL)
    {
        return -1.0f;
    }
    b *= -0.5f;
    if (*penetration<SMALL)
    {
        *penetration = 0.0f;
        Min_Clamp(b, 0.0f);
        return (b > length) ? -1.0f : b;
    }

    //
    //-------------------------------------------------------------
    // We know we hit the sphere, so figure out where it first hits
    //-------------------------------------------------------------
    //
    *penetration = 0.5f * Sqrt(*penetration);
    if (b + *penetration < -SMALL)
    {
        return -1.0f;
    }
    b -= *penetration;
    if (b > length)
    {
        return -1.0f;
    }
    Min_Clamp(b, 0.0f);
    return b;
}

void Matrix3DUtils::getDown(const Matrix3D &m, Vector3D &out) {
    m.copyColumnTo(1, out);
    out.negate();
}

Vector3D Vector3D :: projectOnto(Vector3D v) {
    return v.multiply(v.getDotProduct(Vector3D(x,y,z)) / v.getSquaredLength()  );
}

void CSpring::update()
{
	if(status == 1)
	{
		Vector3D springVector;
		springVector = p1->getPos() - p2->getPos();

		double r = springVector.length();

		if((r <= 0.05)||(r >= 2))
		{
			status = 0;
			return;
		}
//Now Spring can be broken;
		
	
		Vector3D force;	
		if(r != 0)
		{
			force = (springVector / r) * (r - length) * (- stiffnessCoefficient);
		}
			
		force += -(p1->getVel() - p2->getVel()) * frictionCoefficient;

		p1->applyForce(force);
		p2->applyForce(-force);

	/*
		//Ground 
		Vector3D p1Pos;
		Vector3D p2Pos;
		Vector3D p1Vel;
		Vector3D p2Vel;
		Vector3D slip;
		double coeff;

		p1Pos = p1->getPos();
		p2Pos = p2->getPos();
		p1Vel = p1->getVel();
		p2Vel = p2->getVel();
		slip = p2Pos - p1Pos;
	
		if(p1Pos.z <= 0)
		{
			p1Vel.z = 0;
			slip.z = 0;	
			if(p1Vel.scaleM(slip) / p1Vel.meas() / slip.meas() > 0.95)
			{
				p1->applyForce(- p1Vel * (0 * GroundFrictionConstant));					
			}
			else
			{
				p1->applyForce(- p1Vel * GroundFrictionConstant);
			}
		
		}

		if(p2Pos.z <= 0)
		{
			p2Vel.z = 0;
			slip.z = 0;
			if(p2Vel.scaleM(slip) / p2Vel.meas() / slip.meas() < - 0.95)
			{
				p2->applyForce(- p2Vel * (0 * GroundFrictionConstant));					
			}
			else
			{
				p2->applyForce(- p2Vel * GroundFrictionConstant);
			}

		}
*/


	}
}

void
  mitk::SlicedGeometry3D::InitializeEvenlySpaced(
  mitk::PlaneGeometry* geometry2D, mitk::ScalarType zSpacing,
  unsigned int slices, bool flipped )
{
  assert( geometry2D != nullptr );
  assert( geometry2D->GetExtent(0) > 0 );
  assert( geometry2D->GetExtent(1) > 0 );

  geometry2D->Register();

  Superclass::Initialize();
  m_Slices = slices;

  BoundingBox::BoundsArrayType bounds = geometry2D->GetBounds();
  bounds[4] = 0;
  bounds[5] = slices;

  // clear and reserve
  PlaneGeometry::Pointer gnull = nullptr;
  m_PlaneGeometries.assign( m_Slices, gnull );

  Vector3D directionVector = geometry2D->GetAxisVector(2);
  directionVector.Normalize();
  directionVector *= zSpacing;

  if ( flipped == false )
  {
    // Normally we should use the following four lines to create a copy of
    // the transform contrained in geometry2D, because it may not be changed
    // by us. But we know that SetSpacing creates a new transform without
    // changing the old (coming from geometry2D), so we can use the fifth
    // line instead. We check this at (**).
    //
    // AffineTransform3D::Pointer transform = AffineTransform3D::New();
    // transform->SetMatrix(geometry2D->GetIndexToWorldTransform()->GetMatrix());
    // transform->SetOffset(geometry2D->GetIndexToWorldTransform()->GetOffset());
    // SetIndexToWorldTransform(transform);

    this->SetIndexToWorldTransform( const_cast< AffineTransform3D * >(
      geometry2D->GetIndexToWorldTransform() ));
  }
  else
  {
    directionVector *= -1.0;
    this->SetIndexToWorldTransform( AffineTransform3D::New());
    this->GetIndexToWorldTransform()->SetMatrix(
      geometry2D->GetIndexToWorldTransform()->GetMatrix() );

    AffineTransform3D::OutputVectorType scaleVector;
    FillVector3D(scaleVector, 1.0, 1.0, -1.0);
    this->GetIndexToWorldTransform()->Scale(scaleVector, true);
    this->GetIndexToWorldTransform()->SetOffset(
      geometry2D->GetIndexToWorldTransform()->GetOffset() );
  }

  mitk::Vector3D spacing;
  FillVector3D( spacing,
    geometry2D->GetExtentInMM(0) / bounds[1],
    geometry2D->GetExtentInMM(1) / bounds[3],
    zSpacing );

  this->SetDirectionVector( directionVector );
  this->SetBounds( bounds );
  this->SetPlaneGeometry( geometry2D, 0 );
  this->SetSpacing( spacing, true);
  this->SetEvenlySpaced();

  //this->SetTimeBounds( geometry2D->GetTimeBounds() );

  assert(this->GetIndexToWorldTransform()
    != geometry2D->GetIndexToWorldTransform()); // (**) see above.

  this->SetFrameOfReferenceID( geometry2D->GetFrameOfReferenceID() );
  this->SetImageGeometry( geometry2D->GetImageGeometry() );

  geometry2D->UnRegister();
}

bool inDaBox(Vector3D centre, float rayonf, Vector3D vertex)
{
	//std::cout << vertex.x() << "  " << vertex.y() << "  " << vertex.z() << std::endl;
	if (vertex.x() >= centre.x() - rayonf &&  vertex.x() < centre.x() + rayonf &&
		vertex.y() >= centre.y() - rayonf &&  vertex.y() < centre.y() + rayonf &&
		vertex.z() >= centre.z() - rayonf &&  vertex.z() < centre.z() + rayonf)
		return true;
	else
		return false;
}

void cube(Vector3D centre, float rayon)
{

	glBegin(GL_LINE_LOOP);
	glColor3f(1.0, 1.0, 1.0);
	glVertex3f(centre.x() - rayon, centre.y() - rayon, centre.z() - rayon);
	glVertex3f(centre.x() - rayon, centre.y() + rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() + rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() - rayon, centre.z() - rayon);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glColor3f(1.0, 1.0, 1.0);
	glVertex3f(centre.x() - rayon, centre.y() - rayon, centre.z() + rayon);
	glVertex3f(centre.x() - rayon, centre.y() + rayon, centre.z() + rayon);
	glVertex3f(centre.x() + rayon, centre.y() + rayon, centre.z() + rayon);
	glVertex3f(centre.x() + rayon, centre.y() - rayon, centre.z() + rayon);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glColor3f(1.0, 1.0, 1.0);
	glVertex3f(centre.x() - rayon, centre.y() - rayon, centre.z() - rayon);
	glVertex3f(centre.x() - rayon, centre.y() + rayon, centre.z() - rayon);
	glVertex3f(centre.x() - rayon, centre.y() + rayon, centre.z() + rayon);
	glVertex3f(centre.x() - rayon, centre.y() - rayon, centre.z() + rayon);
	glEnd();


	glBegin(GL_LINE_LOOP);
	glColor3f(1.0, 1.0, 1.0);
	glVertex3f(centre.x() + rayon, centre.y() - rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() + rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() + rayon, centre.z() + rayon);
	glVertex3f(centre.x() + rayon, centre.y() - rayon, centre.z() + rayon);
	glEnd();



	glBegin(GL_LINE_LOOP);
	glColor3f(1.0, 1.0, 1.0);
	glVertex3f(centre.x() - rayon, centre.y() - rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() - rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() - rayon, centre.z() + rayon);
	glVertex3f(centre.x() - rayon, centre.y() - rayon, centre.z() + rayon);
	glEnd();



	glBegin(GL_LINE_LOOP);
	glColor3f(1.0, 1.0, 1.0);
	glVertex3f(centre.x() - rayon, centre.y() + rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() + rayon, centre.z() - rayon);
	glVertex3f(centre.x() + rayon, centre.y() + rayon, centre.z() + rayon);
	glVertex3f(centre.x() - rayon, centre.y() + rayon, centre.z() + rayon);
	glEnd();
}

Vector3D Vector3D::getVectorTo(const Vector3D &other) {
	Length x = other.getX().minus(_x);
	Length y = other.getY().minus(_y);
	Length z = other.getZ().minus(_z);
	return Vector3D(x, y, z);
}

void mitk::ExtractDirectedPlaneImageFilterNew::ItkSliceExtraction(const itk::Image<TPixel, VImageDimension> *inputImage)
{
  typedef itk::Image<TPixel, VImageDimension> InputImageType;
  typedef itk::Image<TPixel, VImageDimension - 1> SliceImageType;

  typedef itk::ImageRegionConstIterator<SliceImageType> SliceIterator;

  // Creating an itk::Image that represents the sampled slice
  typename SliceImageType::Pointer resultSlice = SliceImageType::New();

  typename SliceImageType::IndexType start;

  start[0] = 0;
  start[1] = 0;

  Point3D origin = m_CurrentWorldPlaneGeometry->GetOrigin();
  Vector3D right = m_CurrentWorldPlaneGeometry->GetAxisVector(0);
  Vector3D bottom = m_CurrentWorldPlaneGeometry->GetAxisVector(1);

  // Calculation the sample-spacing, i.e the half of the smallest spacing existing in the original image
  Vector3D newPixelSpacing = m_ImageGeometry->GetSpacing();
  float minSpacing = newPixelSpacing[0];
  for (unsigned int i = 1; i < newPixelSpacing.Size(); i++)
  {
    if (newPixelSpacing[i] < minSpacing)
    {
      minSpacing = newPixelSpacing[i];
    }
  }

  newPixelSpacing[0] = 0.5 * minSpacing;
  newPixelSpacing[1] = 0.5 * minSpacing;
  newPixelSpacing[2] = 0.5 * minSpacing;

  float pixelSpacing[2];
  pixelSpacing[0] = newPixelSpacing[0];
  pixelSpacing[1] = newPixelSpacing[1];

  // Calculating the size of the sampled slice
  typename SliceImageType::SizeType size;
  Vector2D extentInMM;
  extentInMM[0] = m_CurrentWorldPlaneGeometry->GetExtentInMM(0);
  extentInMM[1] = m_CurrentWorldPlaneGeometry->GetExtentInMM(1);

  // The maximum extent is the lenght of the diagonal of the considered plane
  double maxExtent = sqrt(extentInMM[0] * extentInMM[0] + extentInMM[1] * extentInMM[1]);
  unsigned int xTranlation = (maxExtent - extentInMM[0]);
  unsigned int yTranlation = (maxExtent - extentInMM[1]);
  size[0] = (maxExtent + xTranlation) / newPixelSpacing[0];
  size[1] = (maxExtent + yTranlation) / newPixelSpacing[1];

  // Creating an ImageRegion Object
  typename SliceImageType::RegionType region;

  region.SetSize(size);
  region.SetIndex(start);

  // Defining the image`s extent and origin by passing the region to it and allocating memory for it
  resultSlice->SetRegions(region);
  resultSlice->SetSpacing(pixelSpacing);
  resultSlice->Allocate();

  /*
  * Here we create an new geometry so that the transformations are calculated correctly (our resulting slice has a
  * different bounding box and spacing)
  * The original current worldgeometry must be cloned because we have to keep the directions of the axis vector which
  * represents the rotation
  */
  right.Normalize();
  bottom.Normalize();
  // Here we translate the origin to adapt the new geometry to the previous calculated extent
  origin[0] -= xTranlation * right[0] + yTranlation * bottom[0];
  origin[1] -= xTranlation * right[1] + yTranlation * bottom[1];
  origin[2] -= xTranlation * right[2] + yTranlation * bottom[2];

  // Putting it together for the new geometry
  mitk::BaseGeometry::Pointer newSliceGeometryTest =
    dynamic_cast<BaseGeometry *>(m_CurrentWorldPlaneGeometry->Clone().GetPointer());
  newSliceGeometryTest->ChangeImageGeometryConsideringOriginOffset(true);

  // Workaround because of BUG (#6505)
  newSliceGeometryTest->GetIndexToWorldTransform()->SetMatrix(
    m_CurrentWorldPlaneGeometry->GetIndexToWorldTransform()->GetMatrix());
  // Workaround end

  newSliceGeometryTest->SetOrigin(origin);
  ScalarType bounds[6] = {0, static_cast<ScalarType>(size[0]), 0, static_cast<ScalarType>(size[1]), 0, 1};
  newSliceGeometryTest->SetBounds(bounds);
  newSliceGeometryTest->SetSpacing(newPixelSpacing);
  newSliceGeometryTest->Modified();

  // Workaround because of BUG (#6505)
  itk::MatrixOffsetTransformBase<mitk::ScalarType, 3, 3>::MatrixType tempTransform =
    newSliceGeometryTest->GetIndexToWorldTransform()->GetMatrix();
  // Workaround end

  /*
  * Now we iterate over the recently created slice.
  * For each slice - pixel we check whether there is an according
  * pixel in the input - image which can be set in the slice.
//.........这里部分代码省略.........

Vector3D::Vector3D(const Vector3D &vec) {
	_x = vec.getX();
	_y = vec.getY();
	_z = vec.getZ();
}

void GL3CalculateProjectionShadowMapMatrix(Vector3D viewp, Vector3D light_direction,
Vector3D x_direction, Vector3D y_direction, float zmin, float zmax) {
#if 0
    char *s1 = light_direction.GetString();
    char *s2 = x_direction.GetString();
    char *s3 = y_direction.GetString();
    sreMessage(SRE_MESSAGE_LOG, "GL3CalculateProjectionShadowMapMatrix: light_dir = %s, "
        "x_dir = %s, y_dir = %s, dot products: %f, %f, %f", s1, s2, s3,
        Dot(light_direction, x_direction), Dot(light_direction, y_direction),
        Dot(x_direction, y_direction));
    delete s1;
    delete s2;
    delete s3;
#endif
    Vector3D fvec = light_direction;
    Vector3D s = x_direction;
    Vector3D u = y_direction;
    Matrix4D M;
    // Note that the y direction has to be negated in order to preserve the handedness of
    // triangles when rendering the shadow map.
    M.Set(
        s.x, s.y, s.z, 0,
        - u.x, - u.y, - u.z, 0,
        - fvec.x, - fvec.y, - fvec.z, 0,
        0.0f, 0.0f, 0.0f, 1.0f);
    Matrix4D T;
    T.AssignTranslation(- viewp);
    // Calculate the projection matrix with a field of view of 90 degrees.
    float aspect = 1.0;
    float e = 1 / tanf((90.0 * M_PI / 180) / 2);
    float n = zmin;
    float f = zmax;
    float l = - n / e;
    float r = n / e;
    float b = - (1.0f / aspect) * n / e;
    float t = (1.0f / aspect) * n / e;
    Matrix4D projection_matrix;
    projection_matrix.Set(
        2 * n / (r - l), 0.0f, (r + l) / (r - l), 0.0f,
        0.0f, 2 * n / (t - b), (t + b) / (t - b), 0.0f,
        0.0f, 0.0f, - (f + n) / (f - n), - 2 * n * f / (f - n),
        0.0f, 0.0f, - 1.0f, 0.0f);
    projection_shadow_map_matrix = projection_matrix * (M * T);
    MatrixTransform shadow_map_viewport_matrix;
    shadow_map_viewport_matrix.Set(
        0.5f, 0.0f, 0.0f, 0.5f,
        0.0f, 0.5f, 0.0f, 0.5f,
        0.0f, 0.0f, 0.5f, 0.5f);
    projection_shadow_map_lighting_pass_matrix = shadow_map_viewport_matrix *
        projection_shadow_map_matrix;
//    projection_shadow_map_lighting_pass_matrix = projection_shadow_map_matrix;

#if 0
    Point3D P1 = viewp + light_direction * n;
    Point3D P2 = viewp + light_direction * f;
    Point3D P3 = viewp + light_direction * f + x_direction * f + y_direction * f;
    Vector4D P1_proj = projection_shadow_map_matrix * P1;
    Vector4D P2_proj = projection_shadow_map_matrix * P2;
    Vector4D P3_proj = projection_shadow_map_matrix * P3;
    Vector3D P1_norm = P1_proj.GetVector3D() / P1_proj.w;
    Vector3D P2_norm = P2_proj.GetVector3D() / P2_proj.w;
    Vector3D P3_norm = P3_proj.GetVector3D() / P3_proj.w;
    char *P1_norm_str = P1_norm.GetString();
    char *P2_norm_str = P2_norm.GetString();
    char *P3_norm_str = P3_norm.GetString();
    sreMessage(SRE_MESSAGE_LOG, "CalculateProjectionShadowMapMatrix: Point transformations "
        "%s, %s and %s.", P1_norm_str, P2_norm_str, P3_norm_str);
    delete P1_norm_str;
    delete P2_norm_str;
    delete P3_norm_str;
#endif
}

double mitk::BaseGeometry::GetDiagonalLength2() const
{
  Vector3D diagonalvector = GetCornerPoint()-GetCornerPoint(false, false, false);
  return diagonalvector.GetSquaredNorm();
}

Vector3D reflectionDirection(const Vector3D& incomingVec, const Vector3D& normalVec) {
    Vector3D vout = incomingVec - 2.0*(DotProduct(incomingVec, normalVec))*normalVec;
    return vout.Normalize();
}

//------------------------------------------------------------------------------
//! Computes the Jacobian for all Edges Internal and Boundary
//! Note: Jacobian is computed using first order Q's
//------------------------------------------------------------------------------
void Compute_Jacobian_Approximate_Roe(int AddTime, int Iteration) {
    int i, j, k, iNode, iEdge, ibEdge;
    int node_L, node_R;
    int idgn, idgnL, idgnR, ofdgnL, ofdgnR;
    double area;
    double Q_L[NEQUATIONS];
    double Q_R[NEQUATIONS];
    double **dFdL;
    double **dFdR;
    double **ARoe;
    Vector3D areavec;
    
    // Create the Helper Matrix
    dFdL = (double **) malloc(NEQUATIONS*sizeof(double*));
    dFdR = (double **) malloc(NEQUATIONS*sizeof(double*));
    ARoe = (double **) malloc(NEQUATIONS*sizeof(double*));
    for (i = 0; i < NEQUATIONS; i++) {
        dFdL[i] = (double *) malloc(NEQUATIONS*sizeof(double));
        dFdR[i] = (double *) malloc(NEQUATIONS*sizeof(double));
        ARoe[i] = (double *) malloc(NEQUATIONS*sizeof(double));
    }
    
    // Initialize the CRS Matrix
    for (i = 0; i < SolverBlockMatrix.DIM; i++) {
        for (j = 0; j < SolverBlockMatrix.Block_nRow; j++) {
            for (k = 0; k < SolverBlockMatrix.Block_nCol; k++)
                SolverBlockMatrix.A[i][j][k] = 0.0;
        }
    }
    
    // Copy the Residuals to Block Matrix which is B
    // And Copy I/DeltaT
    for (iNode = 0; iNode < nNode; iNode++) {
        // Get the diagonal location
        idgn = SolverBlockMatrix.IAU[iNode];
        // Get the LHS
        SolverBlockMatrix.B[iNode][0] = -Res1[iNode];
        SolverBlockMatrix.B[iNode][1] = -Res2[iNode];
        SolverBlockMatrix.B[iNode][2] = -Res3[iNode];
        SolverBlockMatrix.B[iNode][3] = -Res4[iNode];
        SolverBlockMatrix.B[iNode][4] = -Res5[iNode];
        for (j = 0; j < SolverBlockMatrix.Block_nRow; j++) {
            if (AddTime == 1) {
                for (k = 0; k < SolverBlockMatrix.Block_nCol; k++)
                    if (k == j)
                        SolverBlockMatrix.A[idgn][j][k] = cVolume[iNode]/DeltaT[iNode];
            }
        }
    }
    
    // Internal Edges
    for (iEdge = 0; iEdge < nEdge; iEdge++) {
        // Get two nodes of edge
        node_L = intEdge[iEdge].node[0];
        node_R = intEdge[iEdge].node[1];
        
        // Get area vector
        areavec = intEdge[iEdge].areav;
        area    = areavec.magnitude();
        
        // Backup the Copy of Q_L and Q_R
        // Left
        Q_L[0] = Q1[node_L];
        Q_L[1] = Q2[node_L];
        Q_L[2] = Q3[node_L];
        Q_L[3] = Q4[node_L];
        Q_L[4] = Q5[node_L];
        // Right
        Q_R[0] = Q1[node_R];
        Q_R[1] = Q2[node_R];
        Q_R[2] = Q3[node_R];
        Q_R[3] = Q4[node_R];
        Q_R[4] = Q5[node_R];
        
        // Get the diagonal Locations
        idgnL = SolverBlockMatrix.IAU[node_L];
        idgnR = SolverBlockMatrix.IAU[node_R];
        
        // Get the Off-Diagonal Locations
        // node_L: ofdgnL-> node_R;
        // node_R: ofdgnR-> node_L;
        ofdgnL = -1;
        for (i = SolverBlockMatrix.IA[node_L]; i < SolverBlockMatrix.IA[node_L+1]; i++) {
            if (SolverBlockMatrix.JA[i] == node_R) {
                ofdgnL = i;
                break;
            }
        }
        ofdgnR = -1;
        for (i = SolverBlockMatrix.IA[node_R]; i < SolverBlockMatrix.IA[node_R+1]; i++) {
            if (SolverBlockMatrix.JA[i] == node_L) {
                ofdgnR = i;
                break;
            }
        }
        
//.........这里部分代码省略.........

void Object3D::getBackVector(Vector3D& result)
{
	getForwardVector(result);
	result.negate();
}

void subdivision(Vector3D centre, float rayon, float rayonMin)
{
	std::vector<Vector3D> lesCentres;
	if (rayon > rayonMin)
	{
		lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() - rayon / 2, centre.z() - rayon / 2));
		lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() + rayon / 2, centre.z() - rayon / 2));
		lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() - rayon / 2, centre.z() - rayon / 2));
		lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() + rayon / 2, centre.z() - rayon / 2));

		lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() - rayon / 2, centre.z() + rayon / 2));
		lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() + rayon / 2, centre.z() + rayon / 2));
		lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() - rayon / 2, centre.z() + rayon / 2));
		lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() + rayon / 2, centre.z() + rayon / 2));

		for (int i = 0; i < lesCentres.size(); i++)
		{
			subdivision(lesCentres.at(i), rayon / 2, rayonMin);
		}
	}
	else
	{
		finalSize = rayon;
		centreCube.push_back(centre);
		//cube(centre, rayon);
	}
}

GLvoid afficheMaille()
{
	Vector3D a;
	Vector3D b;
	Vector3D c;
	double facteur = 100.0;
	int cptNormale = 0;

	std::vector<Vector3D> geometrie = maille.getGeom();
	std::vector<Vector3D> normales = maille.getNormales();
	std::vector<int> topologie = maille.getTopo();
	Vector3D centre = maille.getCentreGravite();
	double scale = maille.getScale();

	
	//std::cout << maille.getTopo().size() << std::endl;
	for (int i = 0; i < topologie.size(); i += 3)
	{
		a = (geometrie.at(topologie.at(i)));// -centre) / scale;
		b = (geometrie.at(topologie.at(i + 1)));// -centre) / scale;
		c = (geometrie.at(topologie.at(i + 2)));// -centre) / scale;

		//std::cout << "a : " << a.x() << " | " <<  a.y()<< " | " <<  a.z() << std::endl;
		//std::cout << "b : " << b.x() << " | " <<  b.y()<< " | " <<  b.z() << std::endl;
		//std::cout << "c : " << c.x() << " | " <<  c.y()<< " | " <<  c.z() << std::endl;

		//glNormal3d(normales.at(cptNormale).x(), normales.at(cptNormale).y(), normales.at(cptNormale).z());
		//std::cout << normales.at(cptNormale).x() << " | " <<  normales.at(cptNormale).y()<< " | " <<  normales.at(cptNormale).z() << std::endl;
		glBegin(GL_TRIANGLES);
		glVertex3f(a.x(), a.y(), a.z());
		glVertex3f(b.x(), b.y(), b.z());
		glVertex3f(c.x(), c.y(), c.z());
		glEnd();

		cptNormale++;
	}

	//std::cout << "endload" << std::endl;
}

void Plant::draw( Vector3D *inPosition, double inScale,
                  double inMaxZ, double inMinZ ) {


    if( mPoisoned && mPoisonStatus >= 1) {
        // draw nothing
        return;
        }

    
    double drawScale = inScale;

    if( mPoisoned ) {
        // shrink with poisoning
        
        drawScale *= ( 1 - mPoisonStatus );
        }

    
    double radius = drawScale * ( mGrowth * 0.8 + 0.2 );


    // leaves become black with poisoning
    // (shades of white to allow texture color to dominate) 
    Color leafColor( 1 - mPoisonStatus,
                     1 - mPoisonStatus,
                     1 - mPoisonStatus, 1 );

    
    if( ! Features::drawNicePlantLeaves ) {
        // set color to shades of green green for leaves if we're drawing
        // simple boxes, since there's no texture color

        leafColor.setValues( 0, 1 - mPoisonStatus, 0, 1 );
        }

    
    Angle3D zeroAngle( 0, 0, 0 );
    

    PlantGenetics *genetics = &( mSeeds.mGenetics );

    
    
    int maxNumJoints = (int)( genetics->getParameter( jointCount ) );

    double growthFactor = mGrowth * 0.8 + 0.2;
    
    int numFullJoints = (int)( growthFactor * maxNumJoints );

    double partialJoint = growthFactor * maxNumJoints - numFullJoints;

    int numLeavesPerJoint = (int)( genetics->getParameter( leavesPerJoint ) );

    Angle3D angleIncrement( 0, 0, 2 * M_PI / numLeavesPerJoint );
    Angle3D startAngle( 0, 0, mStartZAngle );

    double currentScale = 1;

    double scaleDecrement = currentScale / ( maxNumJoints + 1 );

    Vector3D leafPosition( inPosition );

    Vector3D positionIncrement( 0, 0, -0.5 );

    Vector3D leafWalkerTerminus;

    SimpleVector<Vector3D *> thisLayerLeafTerminii;
    
    
    for( int j=0; j<numFullJoints; j++ ) {

        // lower leaves are darker
        double colorScaleFactor = (double)(j+1) / (double)maxNumJoints; 
        // min scaling of 0.5
        colorScaleFactor = colorScaleFactor * 0.5 + 0.5;
        
        
        Color thisLevelColor;
        thisLevelColor.setValues( &leafColor );
        thisLevelColor.weightColor( colorScaleFactor );
        
        
        
        
        Angle3D currentAngle( &startAngle );

        double zValue = leafPosition.mZ;

        if( zValue <= inMaxZ && zValue >= inMaxZ ) {
            // draw this joint
            for( int g=0; g<numLeavesPerJoint; g++ ) {

                if( Features::drawShadows ) {
                    // draw shadow
                    glColor4f( 0, 0, 0, 0.5 );
                    mLeaf.draw( &leafPosition, &currentAngle,
                                currentScale * radius * 1.05 );
                    }
                
//.........这里部分代码省略.........

//.........这里部分代码省略.........


      // Make deep copy of current Geometry3D of the plane
      data->UpdateOutputInformation(); // make sure that the Geometry is up-to-date
      m_OriginalGeometry = static_cast< BaseGeometry * >(
        data->GetGeometry( timeStep )->Clone().GetPointer() );

      ok = true;
      break;
    }

  case AcMOVE:
    {
      // Check if we have a DisplayPositionEvent
      const DisplayPositionEvent *dpe =
        dynamic_cast< const DisplayPositionEvent * >( stateEvent->GetEvent() );
      if ( dpe == NULL )
      {
        ok = true;
        break;
      }

      if ( currentVtkRenderer != NULL )
      {
        vtkInteractorObserver::ComputeDisplayToWorld(
          currentVtkRenderer,
          m_CurrentPickedDisplayPoint[0],
          m_CurrentPickedDisplayPoint[1],
          0.0, //m_InitialInteractionPickedPoint[2],
          m_CurrentPickedPointWorld );
      }


      Vector3D interactionMove;
      interactionMove[0] = m_CurrentPickedPointWorld[0] - m_InitialPickedPointWorld[0];
      interactionMove[1] = m_CurrentPickedPointWorld[1] - m_InitialPickedPointWorld[1];
      interactionMove[2] = m_CurrentPickedPointWorld[2] - m_InitialPickedPointWorld[2];

      if ( m_InteractionMode == INTERACTION_MODE_TRANSLATION )
      {
        Point3D origin = m_OriginalGeometry->GetOrigin();

        Vector3D transformedObjectNormal;
        data->GetGeometry( timeStep )->IndexToWorld(
          m_ObjectNormal, transformedObjectNormal );

        data->GetGeometry( timeStep )->SetOrigin(
          origin + transformedObjectNormal * (interactionMove * transformedObjectNormal) );
      }
      else if ( m_InteractionMode == INTERACTION_MODE_ROTATION )
      {
        if ( camera )
        {
          double vpn[3];
          camera->GetViewPlaneNormal( vpn );

          Vector3D viewPlaneNormal;
          viewPlaneNormal[0] = vpn[0];
          viewPlaneNormal[1] = vpn[1];
          viewPlaneNormal[2] = vpn[2];

          Vector3D rotationAxis =
            itk::CrossProduct( viewPlaneNormal, interactionMove );
          rotationAxis.Normalize();

          int *size = currentVtkRenderer->GetSize();

void CMuscle::draw()
{

	double pi = 3.14159;
//	double colorCoef;
	GLUquadricObj *quadObj; 
	quadObj = gluNewQuadric(); 

	/*
	if(strength >= income - threshold)
	{
		colorCoef = (income > threshold) * (income - threshold) / strength;
	}
	else
	{
		colorCoef = 1;
	}
	*/

	//if(activity == 1)
	//{
		//glColor3ub(250 / MStrength * (income - threshold) , 0, 0);
	//}
	//else 
	//{
	//	glColor3ub(50 + (int)(150 *colorCoef), 70, 70);
	//}

	float act;
	int r,g,b;
	r = 80;
	g = 80;
	b = 80;
	/*
	double dl = (getP1Pos()-getP2Pos()).length() - getLength0();
	
	if( (act = getActivity())<=1 || dl < 0.0) 
	{

		r += (int)((float)r*2.f*act);
	}
	*/
	r += (255-80)*min(getActivity(),1.f);

	/**/
	//double dl = (getP1Pos()-getP2Pos()).length() - getLength0();
	//if( dl < 0.0) g+= -dl*300;
	/*if( dl < 0.0) { r+= -(int)(dl*500); g+= (int)dl*200; b += (int)dl*200; }
	if(r>255) r = 255;
	if(g<0) g = 0;
	if(b<0) b = 0;*/
	/**/
		//(i_p1->getPos() - i_p2->getPos()).length()
	
	glColor3ub(r,g,b);
	//Vector3D rp1 = (ort1*p1->getX() + ort2*p1->getY() + ort3*p1->getZ())*scale + vcenter;
	Vector3D rp1 = (ort1*(p1->getX()-pos_rc.x) + ort2*(p1->getY()-pos_rc.y) + ort3*(p1->getZ()-pos_rc.z))*scale + vcenter;
	//Vector3D rp2 = (ort1*p2->getX() + ort2*p2->getY() + ort3*p2->getZ())*scale + vcenter;
	Vector3D rp2 = (ort1*(p2->getX()-pos_rc.x) + ort2*(p2->getY()-pos_rc.y) + ort3*(p2->getZ()-pos_rc.z))*scale + vcenter;

	Vector3D p = rp2 - rp1;
	double h = p.length();// /2;

	glPushMatrix();
	
	glTranslated(rp1.x, rp1.y, rp1.z);
	glRotated(180.0f*(double)atan2(p.x,p.z)/pi,0,1,0); 
	glRotated(-180.0f*(double)atan2(p.y,sqrt(p.x*p.x+p.z*p.z))/pi,1,0,0); 
	
	if(mode==0)
		gluCylinder(quadObj, 0, 0.02*scale, h, 16, 16);
	else
		gluCylinder(quadObj, 0, 0.05*scale, h, 16, 16);

	glPopMatrix();

	//p = p1->getPos() - p2->getPos();
	/*
	p = rp1 - rp2;

	glPushMatrix();

	glTranslated(rp2.x, rp2.y, rp2.z);
	glRotated(180.0f*(double)atan2(p.x,p.z)/pi,0,1,0); 
	glRotated(-180.0f*(double)atan2(p.y,sqrt(p.x*p.x+p.z*p.z))/pi,1,0,0);

	gluCylinder(quadObj, 0, 0.05*scale, h, 16, 16);

	glPopMatrix();
	*/

	gluDeleteQuadric(quadObj);
}