C++ Array_::size方法代码示例

void ContactGeometry::TriangleMesh::Impl::splitObbAxis
   (const Array_<int>& parentIndices, Array_<int>& child1Indices,
    Array_<int>& child2Indices, int axis)
{   // For each face, find its minimum and maximum extent along the axis.
    Vector minExtent(parentIndices.size());
    Vector maxExtent(parentIndices.size());
    for (int i = 0; i < (int) parentIndices.size(); i++) {
        int* vertexIndices = faces[parentIndices[i]].vertices;
        Real minVal = vertices[vertexIndices[0]].pos[axis];
        Real maxVal = vertices[vertexIndices[0]].pos[axis];
        minVal = std::min(minVal, vertices[vertexIndices[1]].pos[axis]);
        maxVal = std::max(maxVal, vertices[vertexIndices[1]].pos[axis]);
        minExtent[i] = std::min(minVal, vertices[vertexIndices[2]].pos[axis]);
        maxExtent[i] = std::max(maxVal, vertices[vertexIndices[2]].pos[axis]);
    }

    // Select a split point that tries to put as many faces as possible
    // entirely on one side or the other.

    Real split = (median(minExtent)+median(maxExtent)) / 2;

    // Choose a side for each face.

    for (int i = 0; i < (int) parentIndices.size(); i++) {
        if (maxExtent[i] <= split)
            child1Indices.push_back(parentIndices[i]);
        else if (minExtent[i] >= split)
            child2Indices.push_back(parentIndices[i]);
        else if (0.5*(minExtent[i]+maxExtent[i]) <= split)
            child1Indices.push_back(parentIndices[i]);
        else
            child2Indices.push_back(parentIndices[i]);
    }
}

Real CylinderImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
    if (derivComponents.size() == 1 && derivComponents[0] < 2)
        return -2*x[derivComponents[0]]/square(ownerp->getRadius());
    if (derivComponents.size() == 2 &&
        derivComponents[0] == derivComponents[1] &&
        derivComponents[0] < 2 )
        return -2/square(ownerp->getRadius());
    return 0;
}

Real ObservedPointFitter::findBestFit
   (const MultibodySystem& system, State& state, 
    const Array_<MobilizedBodyIndex>&  bodyIxs, 
    const Array_<Array_<Vec3> >&       stations, 
    const Array_<Array_<Vec3> >&       targetLocations, 
    Real                                        tolerance) 
{
    Array_<Array_<Real> > weights(stations.size());
    for (int i = 0; i < (int)stations.size(); ++i)
        for (int j = 0; j < (int)stations[i].size(); ++j)
            weights[i].push_back(1.0);
    return findBestFit(system, state, bodyIxs, stations, targetLocations, weights, tolerance);
}

Real EllipsoidImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
    const Vec3& radii = ownerp->getRadii();
    if (derivComponents.size() == 1) {
        int c = derivComponents[0];
        return -2*x[c]/(radii[c]*radii[c]);
    }
    if (   derivComponents.size() == 2 
        && derivComponents[0] == derivComponents[1]) {
        int c = derivComponents[0];
        return -2/(radii[c]*radii[c]);
    }
    // A mixed second derivative, or any higher derivative is zero.
    return 0;
}

int ObservedPointFitter::findBodiesForClonedSystem(MobilizedBodyIndex primaryBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs) {
    findUpstreamBodies(primaryBodyIx, numStations,  matter, bodyIxs, 5);
    int primaryBodyIndex = bodyIxs.size();
    int requiredStations = 5;
    findDownstreamBodies(primaryBodyIx, numStations, children, bodyIxs, requiredStations);
    return primaryBodyIndex;
}

void PerSubsystemInfo::
popAllocationStackBackToStage(Array_<T>& stack, const Stage& g) {
    unsigned newSize = stack.size();
    while (newSize > 0 && stack[newSize-1].getAllocationStage() > g)
        stack[--newSize].deepDestruct(*m_stateImpl);
    stack.resize(newSize); 
}

    Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const
    {
        Real deriv = 0;
    
        assert(3 == x.size());
        
        int derivOrder = derivComponents.size();
        assert(1 <= derivOrder);

        if (derivOrder == 1) 
        {
            // too clever
            //    df/dx
            //  = 2(x-y) + 2(x-z) 
            //  = 2x - 2y + 2x - 2z
            //  = 4x - 2y - 2z
            //  = 6x - 2x - 2y - 2z
            deriv = 2 * (3 * x[derivComponents[0]] -x[0] -x[1] -x[2]);
        }
        else if (derivOrder == 2) 
        {
            if (derivComponents[0] == derivComponents[1])
                deriv = 4.0; // df/dx^2
            else
                deriv = -2.0; // df/dxdy
        } 
        else ; // all derivatives higher than two are zero

        return deriv;
    }

Real TorusImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
    // first derivatives
    if (derivComponents.size() == 1) {
        if (derivComponents[0]<2) {
            Real sqrt_xy = std::sqrt(x[0]*x[0] + x[1]*x[1]);
            return 2*x[derivComponents[0]]*(ownerp->getTorusRadius() - sqrt_xy)/
                    (square(ownerp->getTubeRadius())*sqrt_xy);
        }
        else
            return -2*x[2]/square(ownerp->getTubeRadius());
    }

    // second derivatives
    if (derivComponents.size() == 2) {
        if (derivComponents[0] < 2) { // fx_ fy_
            if (derivComponents[1] < 2) {
                Real tubeRadiusSq = square(ownerp->getTubeRadius());
                Real xy = x[0]*x[0] + x[1]*x[1];
                Real sqrt_xy = std::sqrt(xy);
                Real den = tubeRadiusSq*xy*sqrt_xy;
                if (derivComponents[0]==derivComponents[1]) { // fxx or fyy
                    int idx = derivComponents[1]==0; // if 0 then idx=1, if 1 then idx=0
                    Real num = 2*ownerp->getTorusRadius()*x[idx]*x[idx];
                    return num/den - 2/tubeRadiusSq;
                }
                else { // fxy or fyx
                    return - 2*ownerp->getTorusRadius()*x[0]*x[1]/den;
                }
            }
            else // fxz = fyz = 0
                return 0;
        }
        else { // fz_
            if (derivComponents[1] == 2) // fzz
                return -2/square(ownerp->getTubeRadius());
            else // fzx = fzy = 0
                return 0;
        }
    }

    //TODO higher order derivatives
    SimTK_ASSERT1_ALWAYS(!"derivative not implemented",
        "Implicit Torus implements 1st&2nd derivs only but %d deriv requested.",
        derivComponents.size());
    return 0;
}

 NumJacobianFunc(Assembler& assembler,
                 const Array_<AssemblyConditionIndex>& numCons,
                 const Array_<int>& nErrorEqns,
                 int totalNEqns)
 :   Differentiator::JacobianFunction
         (totalNEqns, assembler.getNumFreeQs()),
     assembler(assembler), numCons(numCons), nEqns(nErrorEqns),
     totalNEqns(totalNEqns)
 {   assert(numCons.size() == nEqns.size()); }

 void calcVelocityDotErrors     
    (const State&                                    s,
     const Array_<SpatialVec,ConstrainedBodyIndex>&  A_AB, 
     const Array_<Real,      ConstrainedUIndex>&     constrainedUDot,
     Array_<Real>&                                   vaerr) const override
 {
     assert(vaerr.size() == 1);
     vaerr[0] = getOneUDot(s, constrainedUDot,
                           theMobilizer, whichMobility);
 }

int PolygonalMesh::addFace(const Array_<int>& vertices) {
    initializeHandleIfEmpty();
    for (int i = 0; i < (int) vertices.size(); i++)
        updImpl().faceVertexIndex.push_back(vertices[i]);

    // faceVertexStart is preloaded to have its first element 0 before any
    // faces have been added. So the back() element of faceVertexStart is
    // already the starting entry for the face we're adding.
    // This is where the *next* face will begin.
    updImpl().faceVertexStart.push_back(getImpl().faceVertexIndex.size());
    // The current face start is now at end()-2 (back()-1).
    return getImpl().faceVertexStart.size()-2;
}

// We also rummage through the model to find fixed geometry that should be part
// of every frame. The supplied State must be realized through Instance stage.
void ModelVisualizer::collectFixedGeometry(const State& state) const {
    // Collect any fixed geometry from the ModelComponents.
    Array_<DecorativeGeometry> fixedGeometry;
    _model.generateDecorations
       (true, _model.getDisplayHints(), state, fixedGeometry);

    for (unsigned i=0; i < fixedGeometry.size(); ++i) {
        const DecorativeGeometry& dgeo = fixedGeometry[i];
        //cout << dgeo.getBodyId() << dgeo.getTransform() << endl;
        _viz->addDecoration(MobilizedBodyIndex(dgeo.getBodyId()), 
                            Transform(), dgeo);
    }
}

void testMoveConstructionAndAssignment() {
    Array_<double> ad1{1,2,3.5,4};
    const double* p1 = ad1.data();
    Array_<double> ad2{.01,.02};
    const double* p2 = ad2.data();

    Array_<double> ad3(ad1); // copy construction
    const double* p3 = ad3.data();
    SimTK_TEST(p3 != p2);
    ad3 = std::move(ad1);    // move assignment
    SimTK_TEST(ad3.data() == p1 && ad1.data() == p3);

    Array_<double> ad4(std::move(ad2)); // move construction
    SimTK_TEST(ad4.data()==p2 && ad2.empty());

    auto returned = returnByValue(3.25); // construction
    SimTK_TEST(returned.first == std::vector<double>({1,2,3,4,5.5,3.25}));
    SimTK_TEST(returned.first.data() == returned.second);

    returned = returnByValue(-1);       // assignment
    SimTK_TEST(returned.first == std::vector<double>({1,2,3,4,5.5,-1}));
    SimTK_TEST(returned.first.data() == returned.second);

    // std::unique_ptr has only move construction so this won't compile if
    // Array_ requires copy construction
    Array_<std::unique_ptr<double>> aud;
    aud.push_back(std::unique_ptr<double>(new double(5.125)));
    aud.push_back(std::unique_ptr<double>(new double(3.5)));
    aud.push_back(std::unique_ptr<double>(new double(-2.25)));
    SimTK_TEST(aud.size()==3);
    SimTK_TEST(*aud[0]==5.125 && *aud[1]==3.5 && *aud[2]==-2.25);

    aud.emplace_back(new double(123.));
    SimTK_TEST(aud.size()==4 && *aud[3]==123.);

    aud.emplace(&aud[2], new double(100));
    SimTK_TEST(aud.size()==5 && *aud[2]==100. && *aud[3]==-2.25);
}

/** Same as above but for a given time series */
void InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, const Array_<double> &times, Array_<Vector> &genForceTrajectory)
{
	int nq = getModel().getNumCoordinates();
	int nt = times.size();

	//Preallocate if not done already
	genForceTrajectory.resize(nt, Vector(nq));
	
	AnalysisSet& analysisSet = const_cast<AnalysisSet&>(getModel().getAnalysisSet());
	//fill in results for each time
	for(int i=0; i<nt; i++){ 
		genForceTrajectory[i] = solve(s, Qs, times[i]);
		analysisSet.step(s, i);
	}
}

void Assembler::initialize() const {
    if (alreadyInitialized)
        return;

    ++nInitializations;

    Array_<QIndex> toBeLocked;
    reinitializeWithExtraQsLocked(toBeLocked);
    alreadyInitialized = true;
    return;

    /*NOTREACHED*/
    // TODO: This currently unused code would allow the Assembler to lock out
    // variables that it thinks aren't worth bothering with. Needs real-world
    // testing and probably some override options. And should there be a
    // desperation mode where all variables are tried if we can't assemble
    // with some of them removed?
    Vector grad = abs(asmSys->calcCurrentGradient());
    Real maxGrad = 0;
    for (FreeQIndex fx(0); fx < grad.size(); ++fx)
        maxGrad = std::max(maxGrad, grad[fx]);
    if (maxGrad == 0) // no q does anything; probably no objective
        maxGrad = Infinity; // no q will be kept for gradient purposes

    Matrix jac = asmSys->calcCurrentJacobian();
    Vector colNorm(getNumFreeQs());
    Real maxJac = 0;
    for (FreeQIndex fx(0); fx < grad.size(); ++fx) {
        colNorm[fx] = jac(fx).norm();
        maxJac = std::max(maxJac, colNorm[fx]);
    }
    if (maxJac == 0) // no q does anything; probably no constraints
        maxJac = Infinity; // no q will be kept for Jacobian purposes

    const Real QTol = SqrtEps;
    const Real minGradAllowed = maxGrad*QTol;
    const Real minJacAllowed = maxJac*QTol;
    for (FreeQIndex fx(0); fx < grad.size(); ++fx)
        if (grad[fx] < minGradAllowed && colNorm[fx] < minJacAllowed)
            toBeLocked.push_back(getQIndexOfFreeQ(fx));

    if (toBeLocked.size()) {
        cout << "Reinitializing with these q's locked:\n";
        cout << toBeLocked << endl;
        reinitializeWithExtraQsLocked(toBeLocked);
        alreadyInitialized = true;
    }
}