C++ Basis类代码示例

double Fields::computeADMMass() {
  Basis* thetaBasis = basis.getBasis(SphericalBasis::COORD2);
  const double* xi = thetaBasis->getAbscissas();
  int nR = basis.getRank(SphericalBasis::COORD1);
  const double* r = basis.getCoord(SphericalBasis::COORD1);
  int nTheta = basis.getRank(SphericalBasis::COORD2);
  const double* theta = basis.getCoord(SphericalBasis::COORD2);
  double integrand[nTheta];
  for (int iTheta = 0; iTheta < nTheta; iTheta++) {
    // This formula holds because of the boundary condition.
    double myPsi = psi(r[nR-1], theta[iTheta]);
    integrand[iTheta] = (myPsi - 1.)*
      r[nR-1]*sin(theta[iTheta]);
    // Change to the spectral coordinate to integrate...
    integrand[iTheta] *= 0.5*M_PI*sqrt(1.0 - xi[iTheta]*xi[iTheta]);
  }
  return thetaBasis->integrate(integrand);
}

int DefaultEvaluatorForIntegralOperators<BasisFunctionType, KernelType,
ResultType, GeometryFactory>::farFieldQuadOrder(
        const Basis<BasisFunctionType>& basis) const
{
    int elementOrder = (basis.order());
    // Order required for exact quadrature on affine elements with kernel
    // approximated by a polynomial of order identical with that of the basis
    int defaultQuadratureOrder = 2 * elementOrder;
    return m_quadratureOptions.quadratureOrder(defaultQuadratureOrder);
}

	//constraint between two different rigidbodies
	btVehicleJacobianEntry(
			const Basis &world2A,
			const Basis &world2B,
			const Vector3 &rel_pos1,
			const Vector3 &rel_pos2,
			const Vector3 &jointAxis,
			const Vector3 &inertiaInvA,
			const real_t massInvA,
			const Vector3 &inertiaInvB,
			const real_t massInvB) :
			m_linearJointAxis(jointAxis) {
		m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis));
		m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis));
		m_0MinvJt = inertiaInvA * m_aJ;
		m_1MinvJt = inertiaInvB * m_bJ;
		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);

		//btAssert(m_Adiag > real_t(0.0));
	}

NeutronDrop::NeutronDrop(Basis &_basis, Interaction &_inter, int _nbNeut, double _omega, int _nPoints) :
  System(std::string("NeutronDrop"), _basis, arma::ivec(
{
  _nbNeut
}), std::vector<std::string>({"neutron"}), _inter, _nPoints), omega(_omega),TBME(_basis.size, _basis.size)

{
  if(basis->type == "SpBasis" || basis->type == "ReducedSpBasis")
  {
     for (int i = 0; i < basis->size; i++)
      for (int j = 0; j < basis->size; j++)
      {
        TBME(i, j) = arma::zeros(_basis.size, basis->size);
        for (int k = 0; k < basis->size; k++)
          for (int l = 0; l < basis->size; l++)
          {
            arma::field<arma::mat> dummyR;
            TBME(i, j)(k, l) = inter->get(dummyR, 0, i, 0, l, 0, j, 0, k);
          }
      }
  }
  else if (basis->type == "FullSpBasis")
  {
    for (int i = 0; i < basis->size; i++)
      for (int j = 0; j < basis->size; j++)
      {
        if (_basis.qNumbers(i, 1) != _basis.qNumbers(j, 1)) continue;
        if (_basis.qNumbers(i, 2) != _basis.qNumbers(j, 2)) continue;
        TBME(i, j) = arma::zeros(_basis.size, basis->size);
        
        for (int k = 0; k < basis->size; k++)
          for (int l = 0; l < basis->size; l++)
          {
            if (_basis.qNumbers(k, 1) != _basis.qNumbers(l, 1)) continue;
            if (_basis.qNumbers(k, 2) != _basis.qNumbers(l, 2)) continue;
            arma::field<arma::mat> dummyR;
            TBME(i, j)(k, l) = inter->get(dummyR, 0, i, 0, l, 0, j, 0, k);
          }
      }
  }
}

void GridMapEditor::_update_duplicate_indicator() {

	if (!selection.active || input_action!=INPUT_DUPLICATE) {

		Transform xf;
		xf.basis.set_zero();
		VisualServer::get_singleton()->instance_set_transform(duplicate_instance,xf);
		return;
	}

	Transform xf;
	xf.scale(Vector3(1,1,1)*(Vector3(1,1,1)+(selection.end-selection.begin))*node->get_cell_size());
	xf.origin=(selection.begin+(selection.current-selection.click))*node->get_cell_size();
	Basis rot;
	rot.set_orthogonal_index(selection.duplicate_rot);
	xf.basis = rot * xf.basis;

	VisualServer::get_singleton()->instance_set_transform(duplicate_instance,node->get_global_transform() * xf);


}

static void UE3_QuantizeBasis(Basis & basis)
{
	float handness = basis.handness();

	UE3_QuantizeVector(basis.normal);
	UE3_QuantizeVector(basis.tangent);

	// Reconstruct bitangent as done in the vertex shader:
	basis.bitangent = cross(basis.normal, basis.tangent) * handness;

	// @@ Does the vertex shader normalize normal, tangent, and bitangent?

}

FnMap minimizeLength(const FnWord &u, const Basis &basis)
{

  /* Returns an automorphism phi such that phi(u) has minimal
     length. */

  int r = basis.getRank();
  FnMap phi(r);

  if (u == Id) return phi;
  if (u.length() == 1) return phi;

  int old_norm,new_norm;
  FnWord u_min(u),tmp;
  FnMap whAuto(r);

  QList<WhiteheadData> whAutos = whiteheadAutos(basis);
  QListIterator<WhiteheadData> move(whAutos);

  bool reduced_norm = true;

  while (reduced_norm) {

    old_norm = u_min.length();
    new_norm = old_norm;
    move.toFront();

    while (old_norm <= new_norm && move.hasNext()) {
      whAuto = whitehead(move.next(),basis);
      tmp = whAuto(u_min);
      new_norm = tmp.length();
    }
    if (new_norm < old_norm) {
      u_min = tmp;
      phi = whAuto*phi;
    } else
      reduced_norm = false;
  } // end while (reduced_norm)
 
  // now u_min = phi(u)

  return phi;

} 

// This is doing a simple ear-clipping algorithm that skips invalid triangles. Ideally, we should
// also sort the ears by angle, start with the ones that have the smallest angle and proceed in order.
HalfEdge::Mesh * nv::triangulate(const HalfEdge::Mesh * inputMesh)
{
    HalfEdge::Mesh * mesh = new HalfEdge::Mesh;
    
    // Add all vertices.
    const uint vertexCount = inputMesh->vertexCount();
    for (uint v = 0; v < vertexCount; v++) {
        const HalfEdge::Vertex * vertex = inputMesh->vertexAt(v);
        mesh->addVertex(vertex->pos);
    }

    Array<int> polygonVertices;
    Array<float> polygonAngles;
    Array<Vector2> polygonPoints;

    const uint faceCount = inputMesh->faceCount();
    for (uint f = 0; f < faceCount; f++)
    {
        const HalfEdge::Face * face = inputMesh->faceAt(f);
        nvDebugCheck(face != NULL);

        const uint edgeCount = face->edgeCount();
        nvDebugCheck(edgeCount >= 3);

        polygonVertices.clear();
        polygonVertices.reserve(edgeCount);

        if (edgeCount == 3) {
            // Simple case for triangles.
            for (HalfEdge::Face::ConstEdgeIterator it(face->edges()); !it.isDone(); it.advance())
            {
                const HalfEdge::Edge * edge = it.current();
                const HalfEdge::Vertex * vertex = edge->vertex;
                polygonVertices.append(vertex->id);
            }

            int v0 = polygonVertices[0];
            int v1 = polygonVertices[1];
            int v2 = polygonVertices[2];

            mesh->addFace(v0, v1, v2);
        }
        else {
            // Build 2D polygon projecting vertices onto normal plane.
            // Faces are not necesarily planar, this is for example the case, when the face comes from filling a hole. In such cases
            // it's much better to use the best fit plane.
            const Vector3 fn = face->normal();

            Basis basis;
            basis.buildFrameForDirection(fn);

            polygonPoints.clear();
            polygonPoints.reserve(edgeCount);
            polygonAngles.clear();
            polygonAngles.reserve(edgeCount);

            for (HalfEdge::Face::ConstEdgeIterator it(face->edges()); !it.isDone(); it.advance())
            {
                const HalfEdge::Edge * edge = it.current();
                const HalfEdge::Vertex * vertex = edge->vertex;
                polygonVertices.append(vertex->id);
                
                Vector2 p;
                p.x = dot(basis.tangent, vertex->pos);
                p.y = dot(basis.bitangent, vertex->pos);

                polygonPoints.append(p);
            }
            polygonAngles.resize(edgeCount);

            while (polygonVertices.size() > 2) {
                uint size = polygonVertices.size();

                // Update polygon angles. @@ Update only those that have changed.
                float minAngle = 2 * PI;
                uint bestEar = 0; // Use first one if none of them is valid.
                bool bestIsValid = false;
                for (uint i = 0; i < size; i++) {
                    uint i0 = i; 
                    uint i1 = (i+1) % size; // Use Sean's polygon interation trick.
                    uint i2 = (i+2) % size;

                    Vector2 p0 = polygonPoints[i0];
                    Vector2 p1 = polygonPoints[i1];
                    Vector2 p2 = polygonPoints[i2];

                    float d = clamp(dot(p0-p1, p2-p1) / (length(p0-p1) * length(p2-p1)), -1.0f, 1.0f);
                    float angle = acosf(d);
                    
                    float area = triangleArea(p0, p1, p2);
                    if (area < 0.0f) angle = 2.0f * PI - angle;

                    polygonAngles[i1] = angle;

                    if (angle < minAngle || !bestIsValid) {

                        // Make sure this is a valid ear, if not, skip this point.
                        bool valid = true;
//.........这里部分代码省略.........

Basis Basis::transposed() const {

	Basis tr = *this;
	tr.transpose();
	return tr;
}

//.........这里部分代码省略.........
  map<string, double>::iterator srcTermEnd = SrcTermWeight.end();

  // Bundle up the dependent variables in the way needed for computing
  // the source terms of each reaction
  /*
  Epetra_Vector debugSrcTerm(*OverlapMap);
  map<string, Epetra_Vector*> debugDepVars;
  debugDepVars.insert( pair<string, Epetra_Vector*>(getName(), &u) );
  for( int i = 0; i<numDep; i++ )
    debugDepVars.insert( pair<string, Epetra_Vector*>
                    (myManager->getName(depProblems[i]), &dep[i]) );

  for( srcTermIter = SrcTermWeight.begin(); 
    srcTermIter != srcTermEnd; srcTermIter++) {
                     HMX_PDE &srcTermProb = 
                     dynamic_cast<HMX_PDE&>(
                     myManager->getProblem(srcTermIter->first) );
    std::cout << "Inside problem: \"" << getName() << "\" calling to get source term "
         << "from problem: \"" << srcTermIter->first << "\" :" << std::endl;
    srcTermProb.computeSourceTerm(debugDepVars, debugSrcTerm);
    std::cout << "Resulting source term :" << debugSrcTerm << std::endl;
  }
  */

  int row;
  double alpha = 500.0;
  double xx[2];
  double uu[2]; 
  double uuold[2];
  std::vector<double*> ddep(numDep);
  for( int i = 0; i<numDep; i++)
    ddep[i] = new double[2];
  double *srcTerm = new double[2];
  Basis basis;

  // Bundle up the dependent variables in the way needed for computing
  // the source terms of each reaction
  map<string, double*> depVars;
  depVars.insert( pair<string, double*>(getName(), uu) );
  for( int i = 0; i<numDep; i++ )
    depVars.insert( pair<string, double*>
                    (myManager->getProblemName(depProblems[i]), ddep[i]) );
  // Do a check on this fill
//  map<string, double*>::iterator iter;
//  for( iter = depVars.begin(); iter != depVars.end(); iter++)
//    std::cout << "Inserted ... " << iter->first << "\t" << iter->second << std::endl;
//  std::cout << "--------------------------------------------------" << std::endl;
//  for( iter = depVars.begin(); iter != depVars.end(); iter++)
//	  std::cout << iter->first << "\t" << (iter->second)[0] << ", " 
//               << (iter->second)[1] << std::endl;
//  std::cout << "--------------------------------------------------" << std::endl;

  // Zero out the objects that will be filled
  if ( fillMatrix ) A->PutScalar(0.0);
  if ( fillF ) rhs->PutScalar(0.0);

  // Loop Over # of Finite Elements on Processor
  for (int ne=0; ne < OverlapNumMyNodes-1; ne++) 
  {
    // Loop Over Gauss Points
    for(int gp=0; gp < 2; gp++) 
    {
      // Get the solution and coordinates at the nodes 
      xx[0]=xvec[ne];
      xx[1]=xvec[ne+1];
      uu[0] = u[ne];

Basis Basis::orthonormalized() const {

	Basis c = *this;
	c.orthonormalize();
	return c;
}

Basis Basis::inverse() const {

	Basis inv = *this;
	inv.invert();
	return inv;
}

// Matrix and Residual Fills
bool
Pitchfork_FiniteElementProblem::evaluate(FillType f,
                     const Epetra_Vector* soln,
                     Epetra_Vector* tmp_rhs,
                     Epetra_RowMatrix* tmp_matrix,
                     double jac_coeff,
                     double mass_coeff)
{
  flag = f;

  // Set the incoming linear objects
  if (flag == F_ONLY) {
    rhs = tmp_rhs;
  } else if (flag == MATRIX_ONLY) {
    A = dynamic_cast<Epetra_CrsMatrix*> (tmp_matrix);
    assert(A != NULL);
  } else if (flag == ALL) {
    rhs = tmp_rhs;
    A = dynamic_cast<Epetra_CrsMatrix*> (tmp_matrix);
    assert(A != NULL);
  } else {
    std::cout << "ERROR: Pitchfork_FiniteElementProblem::fillMatrix() - FillType flag is broken" << std::endl;
    throw;
  }

  // Create the overlapped solution and position vectors
  Epetra_Vector u(*OverlapMap);
  Epetra_Vector x(*OverlapMap);

  // Export Solution to Overlap vector
  u.Import(*soln, *Importer, Insert);

  // Declare required variables
  int i,j,ierr;
  int OverlapNumMyElements = OverlapMap->NumMyElements();

  int OverlapMinMyGID;
  if (MyPID==0) OverlapMinMyGID = StandardMap->MinMyGID();
  else OverlapMinMyGID = StandardMap->MinMyGID()-1;

  int row, column;
  double jac;
  double xx[2];
  double uu[2];
  Basis basis;

  // Create the nodal coordinates
  double Length=2.0;
  double dx=Length/((double) NumGlobalElements-1);
  for (i=0; i < OverlapNumMyElements; i++) {
    x[i]=-1.0 + dx*((double) OverlapMinMyGID+i);
  }

  // Zero out the objects that will be filled
  if ((flag == MATRIX_ONLY) || (flag == ALL)) {
    i = A->PutScalar(0.0);
    assert(i == 0);
  }
  if ((flag == F_ONLY)    || (flag == ALL)) {
    i = rhs->PutScalar(0.0);
    assert(i == 0);
  }

  // Loop Over # of Finite Elements on Processor
  for (int ne=0; ne < OverlapNumMyElements-1; ne++) {

    // Loop Over Gauss Points
    for(int gp=0; gp < 2; gp++) {
      // Get the solution and coordinates at the nodes
      xx[0]=x[ne];
      xx[1]=x[ne+1];
      uu[0]=u[ne];
      uu[1]=u[ne+1];
      // Calculate the basis function at the gauss point
      basis.getBasis(gp, xx, uu);

      // Loop over Nodes in Element
      for (i=0; i< 2; i++) {
    row=OverlapMap->GID(ne+i);
    //printf("Proc=%d GlobalRow=%d LocalRow=%d Owned=%d\n",
    //     MyPID, row, ne+i,StandardMap.MyGID(row));
    if (StandardMap->MyGID(row)) {
      if ((flag == F_ONLY)    || (flag == ALL)) {
        (*rhs)[StandardMap->LID(OverlapMap->GID(ne+i))]+=
          +basis.wt*basis.dx
          *((-1.0/(basis.dx*basis.dx))*basis.duu*
        basis.dphide[i]-source_term(basis.uu)*basis.phi[i]);
      }
    }
    // Loop over Trial Functions
    if ((flag == MATRIX_ONLY) || (flag == ALL)) {
      for(j=0;j < 2; j++) {
        if (StandardMap->MyGID(row)) {
          column=OverlapMap->GID(ne+j);
          jac=jac_coeff*basis.wt*basis.dx*
        ((-1.0/(basis.dx*basis.dx))*basis.dphide[j]*basis.dphide[i]
         -source_deriv(basis.uu)*basis.phi[j]*basis.phi[i]) +
        mass_coeff*basis.wt*basis.dx*basis.phi[j]*basis.phi[i];
          ierr=A->SumIntoGlobalValues(row, 1, &jac, &column);
//.........这里部分代码省略.........

void GDAPI godot_basis_set_axis_angle_scale(godot_basis *p_self, const godot_vector3 *p_axis, godot_real p_phi, const godot_vector3 *p_scale) {
	Basis *self = (Basis *)p_self;
	const Vector3 *axis = (const Vector3 *)p_axis;
	const Vector3 *scale = (const Vector3 *)p_scale;
	self->set_axis_angle_scale(*axis, p_phi, *scale);
}

void MobileVRInterface::set_position_from_sensors() {
	_THREAD_SAFE_METHOD_

	// this is a helper function that attempts to adjust our transform using our 9dof sensors
	// 9dof is a misleading marketing term coming from 3 accelerometer axis + 3 gyro axis + 3 magnetometer axis = 9 axis
	// but in reality this only offers 3 dof (yaw, pitch, roll) orientation

	uint64_t ticks = OS::get_singleton()->get_ticks_usec();
	uint64_t ticks_elapsed = ticks - last_ticks;
	float delta_time = (double)ticks_elapsed / 1000000.0;

	// few things we need
	Input *input = Input::get_singleton();
	Vector3 down(0.0, -1.0, 0.0); // Down is Y negative
	Vector3 north(0.0, 0.0, 1.0); // North is Z positive

	// make copies of our inputs
	bool has_grav = false;
	Vector3 acc = input->get_accelerometer();
	Vector3 gyro = input->get_gyroscope();
	Vector3 grav = input->get_gravity();
	Vector3 magneto = scale_magneto(input->get_magnetometer()); // this may be overkill on iOS because we're already getting a calibrated magnetometer reading

	if (sensor_first) {
		sensor_first = false;
	} else {
		acc = scrub(acc, last_accerometer_data, 2, 0.2);
		magneto = scrub(magneto, last_magnetometer_data, 3, 0.3);
	};

	last_accerometer_data = acc;
	last_magnetometer_data = magneto;

	if (grav.length() < 0.1) {
		// not ideal but use our accelerometer, this will contain shakey shakey user behaviour
		// maybe look into some math but I'm guessing that if this isn't available, its because we lack the gyro sensor to actually work out
		// what a stable gravity vector is
		grav = acc;
		if (grav.length() > 0.1) {
			has_grav = true;
		};
	} else {
		has_grav = true;
	};

	bool has_magneto = magneto.length() > 0.1;
	if (gyro.length() > 0.1) {
		/* this can return to 0.0 if the user doesn't move the phone, so once on, it's on */
		has_gyro = true;
	};

	if (has_gyro) {
		// start with applying our gyro (do NOT smooth our gyro!)
		Basis rotate;
		rotate.rotate(orientation.get_axis(0), gyro.x * delta_time);
		rotate.rotate(orientation.get_axis(1), gyro.y * delta_time);
		rotate.rotate(orientation.get_axis(2), gyro.z * delta_time);
		orientation = rotate * orientation;

		tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
	};

	///@TODO improve this, the magnetometer is very fidgity sometimes flipping the axis for no apparent reason (probably a bug on my part)
	// if you have a gyro + accelerometer that combo tends to be better then combining all three but without a gyro you need the magnetometer..
	if (has_magneto && has_grav && !has_gyro) {
		// convert to quaternions, easier to smooth those out
		Quat transform_quat(orientation);
		Quat acc_mag_quat(combine_acc_mag(grav, magneto));
		transform_quat = transform_quat.slerp(acc_mag_quat, 0.1);
		orientation = Basis(transform_quat);

		tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
	} else if (has_grav) {
		// use gravity vector to make sure down is down...
		// transform gravity into our world space
		grav.normalize();
		Vector3 grav_adj = orientation.xform(grav);
		float dot = grav_adj.dot(down);
		if ((dot > -1.0) && (dot < 1.0)) {
			// axis around which we have this rotation
			Vector3 axis = grav_adj.cross(down);
			axis.normalize();

			Basis drift_compensation(axis, acos(dot) * delta_time * 10);
			orientation = drift_compensation * orientation;
		};
	};

	// JIC
	orientation.orthonormalize();

	last_ticks = ticks;
};