C++ Vector3D类代码示例

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
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();
}