/*============================================================================== Main test driver to be used on any muscle model (derived from Muscle) so new cases should be easy to add currently, the test only verifies that the work done by the muscle corresponds to the change in system energy. TODO: Test will fail wih prescribe motion until the work done by this constraint is accounted for. ================================================================================ */ void simulateMuscle( const Muscle &aMuscModel, double startX, double act0, const Function *motion, // prescribe motion of free end of muscle const Function *control, // prescribed excitation signal to the muscle double integrationAccuracy, int testType, double testTolerance, bool printResults) { string prescribed = (motion == NULL) ? "." : " with Prescribed Motion."; cout << "\n******************************************************" << endl; cout << "Test " << aMuscModel.getConcreteClassName() << " Model" << prescribed << endl; cout << "******************************************************" << endl; using SimTK::Vec3; //========================================================================== // 0. SIMULATION SETUP: Create the block and ground //========================================================================== // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 4.0; //Physical properties of the model double ballMass = 10; double ballRadius = 0.05; double anchorWidth = 0.1; // Create an OpenSim model Model model; double optimalFiberLength = aMuscModel.getOptimalFiberLength(); double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength(); double tendonSlackLength = aMuscModel.getTendonSlackLength(); // Use a copy of the muscle model passed in to add path points later PathActuator *aMuscle = aMuscModel.clone(); // Get a reference to the model's ground body Ground& ground = model.updGround(); ground.addDisplayGeometry("box.vtp"); ground.updDisplayer() ->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2 * anchorWidth)); OpenSim::Body * ball = new OpenSim::Body("ball", ballMass, Vec3(0), ballMass*SimTK::Inertia::sphere(ballRadius)); ball->addDisplayGeometry("sphere.vtp"); ball->updDisplayer()->setScaleFactors(Vec3(2 * ballRadius)); // ball connected to ground via a slider along X double xSinG = optimalFiberLength*cos(pennationAngle) + tendonSlackLength; SliderJoint* slider = new SliderJoint("slider", ground, Vec3(anchorWidth / 2 + xSinG, 0, 0), Vec3(0), *ball, Vec3(0), Vec3(0)); CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet(); jointCoordinateSet[0].setName("tx"); jointCoordinateSet[0].setDefaultValue(1.0); jointCoordinateSet[0].setRangeMin(0); jointCoordinateSet[0].setRangeMax(1.0); if (motion != NULL){ jointCoordinateSet[0].setPrescribedFunction(*motion); jointCoordinateSet[0].setDefaultIsPrescribed(true); } // add ball to model model.addBody(ball); model.addJoint(slider); //========================================================================== // 1. SIMULATION SETUP: Add the muscle //========================================================================== //Attach the muscle const string &actuatorType = aMuscle->getConcreteClassName(); aMuscle->setName("muscle"); aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth / 2, 0, 0)); aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius, 0, 0)); ActivationFiberLengthMuscle_Deprecated *aflMuscle = dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle); if (aflMuscle){ // Define the default states for the muscle that has //activation and fiber-length states aflMuscle->setDefaultActivation(act0); aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength()); } else{ ActivationFiberLengthMuscle *aflMuscle2 = dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle); if (aflMuscle2){ // Define the default states for the muscle //that has activation and fiber-length states aflMuscle2->setDefaultActivation(act0); aflMuscle2->setDefaultFiberLength(aflMuscle2 ->getOptimalFiberLength()); } } model.addForce(aMuscle); // Create a prescribed controller that simply //applies controls as function of time PrescribedController * muscleController = new PrescribedController(); if (control != NULL){ muscleController->setActuators(model.updActuators()); // Set the indiviudal muscle control functions //for the prescribed muscle controller muscleController->prescribeControlForActuator("muscle", control->clone()); // Add the control set controller to the model model.addController(muscleController); } // Set names for muscles / joints. Array<string> muscNames; muscNames.append(aMuscle->getName()); Array<string> jointNames; jointNames.append("slider"); //========================================================================== // 2. SIMULATION SETUP: Instrument the test with probes //========================================================================== Array<string> muscNamesTwice = muscNames; muscNamesTwice.append(muscNames.get(0)); cout << "------------\nPROBES\n------------" << endl; int probeCounter = 1; // Add ActuatorPowerProbe to measure work done by the muscle ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1); //muscWorkProbe->setName("ActuatorWork"); muscWorkProbe->setOperation("integrate"); SimTK::Vector ic1(1); ic1 = 9.0; // some arbitary initial condition. muscWorkProbe->setInitialConditions(ic1); model.addProbe(muscWorkProbe); model.setup(); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl; // Add ActuatorPowerProbe to measure power generated by the muscle ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor muscPowerProbe->setName("ActuatorPower"); muscPowerProbe->setOperation("value"); model.addProbe(muscPowerProbe); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl; // Add ActuatorPowerProbe to report the muscle power MINIMUM ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMinimum->setName("ActuatorPowerMinimum"); powerProbeMinimum->setOperation("minimum"); model.addProbe(powerProbeMinimum); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl; // Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMinAbs->setName("ActuatorPowerMinAbs"); powerProbeMinAbs->setOperation("minabs"); model.addProbe(powerProbeMinAbs); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl; // Add ActuatorPowerProbe to report the muscle power MAXIMUM ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMaximum->setName("ActuatorPowerMaximum"); powerProbeMaximum->setOperation("maximum"); model.addProbe(powerProbeMaximum); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl; // Add ActuatorPowerProbe to report the muscle power MAXABS ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMaxAbs->setName("ActuatorPowerMaxAbs"); powerProbeMaxAbs->setOperation("maxabs"); model.addProbe(powerProbeMaxAbs); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl; // Add ActuatorPowerProbe to measure the square of the power generated by the muscle ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor muscPowerSquaredProbe->setName("ActuatorPowerSquared"); muscPowerSquaredProbe->setExponent(2.0); model.addProbe(muscPowerSquaredProbe); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl; // Add JointInternalPowerProbe to measure work done by the joint JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1); jointWorkProbe->setName("JointWork"); jointWorkProbe->setOperation("integrate"); jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0)); model.addProbe(jointWorkProbe); cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl; // Add JointPowerProbe to measure power generated by the joint JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor jointPowerProbe->setName("JointPower"); jointPowerProbe->setOperation("value"); model.addProbe(jointPowerProbe); cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl; // Add ActuatorForceProbe to measure the impulse of the muscle force ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1); impulseProbe->setName("ActuatorImpulse"); impulseProbe->setOperation("integrate"); impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0)); model.addProbe(impulseProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl; // Add ActuatorForceProbe to report the muscle force ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbe->setName("ActuatorForce"); forceProbe->setOperation("value"); model.addProbe(forceProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl; // Add ActuatorForceProbe to report the square of the muscle force ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor forceSquaredProbe->setName("ActuatorForceSquared"); forceSquaredProbe->setExponent(2.0); model.addProbe(forceSquaredProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl; // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice"); forceSquaredProbeTwice->setSumForcesTogether(true); forceSquaredProbeTwice->setActuatorNames(muscNamesTwice); model.addProbe(forceSquaredProbeTwice); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl; // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5 ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved"); double gain1 = 0.5; forceSquaredProbeTwiceScaled->setGain(gain1); model.addProbe(forceSquaredProbeTwiceScaled); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl; // Add ActuatorForceProbe to report -3.5X the muscle force double gain2 = -3.50; ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbeScale->setName("ScaleActuatorForce"); forceProbeScale->setOperation("value"); forceProbeScale->setGain(gain2); model.addProbe(forceProbeScale); cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl; // Add ActuatorForceProbe to report the differentiated muscle force ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbeDiff->setName("DifferentiateActuatorForce"); forceProbeDiff->setOperation("differentiate"); model.addProbe(forceProbeDiff); cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl; // Add SystemEnergyProbe to measure the system KE+PE SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true); sysEnergyProbe->setName("SystemEnergy"); sysEnergyProbe->setOperation("value"); sysEnergyProbe->setComputeKineticEnergy(true); sysEnergyProbe->setComputePotentialEnergy(true); model.addProbe(sysEnergyProbe); cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl; // Add SystemEnergyProbe to measure system power (d/dt system KE+PE) SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor sysPowerProbe->setName("SystemPower"); sysPowerProbe->setDisabled(false); sysPowerProbe->setOperation("differentiate"); model.addProbe(sysPowerProbe); cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl; // Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR"); forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice); //cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl; forceSquaredProbeTwiceReportedIndividually1->setOperation("value"); model.addProbe(forceSquaredProbeTwiceReportedIndividually1); cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl; // Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR"); forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate"); model.addProbe(forceSquaredProbeTwiceReportedIndividually2); cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; // Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR"); forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate"); SimTK::Vector initCondVec(2); initCondVec(0) = 0; initCondVec(1) = 10; forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec); model.addProbe(forceSquaredProbeTwiceReportedIndividually3); cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; cout << "initCondVec = " << initCondVec << endl; /* Since all components are allocated on the stack don't have model own them (and try to free)*/ // model.disownAllComponents(); model.setName("testProbesModel"); cout << "Saving model... " << endl; model.print("testProbesModel.osim"); cout << "Re-loading model... " << endl; Model reloadedModel = Model("testProbesModel.osim"); /* Setup analyses and reporters. */ ProbeReporter* probeReporter = new ProbeReporter(&model); model.addAnalysis(probeReporter); ForceReporter* forceReporter = new ForceReporter(&model); model.addAnalysis(forceReporter); MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model); model.addAnalysis(muscleReporter); model.print("testProbesModel.osim"); model.printBasicInfo(cout); //========================================================================== // 3. SIMULATION Initialization //========================================================================== // Initialize the system and get the default state SimTK::State& si = model.initSystem(); SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si); model.getMultibodySystem().realize(si, SimTK::Stage::Dynamics); model.equilibrateMuscles(si); CoordinateSet& modelCoordinateSet = model.updCoordinateSet(); // Define non-zero (defaults are 0) states for the free joint // set x-translation value modelCoordinateSet[0].setValue(si, startX, true); //Copy the initial state SimTK::State initialState(si); // Check muscle is setup correctly const PathActuator &muscle = dynamic_cast<const PathActuator&>(model.updActuators().get("muscle")); double length = muscle.getLength(si); double trueLength = startX + xSinG - anchorWidth / 2; ASSERT_EQUAL(length / trueLength, 1.0, testTolerance, __FILE__, __LINE__, "testMuscles: path failed to initialize to correct length."); model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0); //cout << "Muscle initial energy = " << Emuscle0 << endl; double Esys0 = model.getMultibodySystem().calcEnergy(si); Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0)); double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si); //cout << "Total initial system energy = " << Esys0 << endl; //========================================================================== // 4. SIMULATION Integration //========================================================================== // Create the integrator SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); integrator.setAccuracy(integrationAccuracy); // Create the manager Manager manager(model, integrator); // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); cout << "\nIntegrating from " << initialTime << " to " << finalTime << endl; // Start timing the simulation const clock_t start = clock(); // simulate manager.integrate(si); // how long did it take? double comp_time = (double)(clock() - start) / CLOCKS_PER_SEC; //========================================================================== // 5. SIMULATION Reporting //========================================================================== double realTimeMultiplier = ((finalTime - initialTime) / comp_time); printf("testMuscles: Realtime Multiplier: %f\n" " : simulation duration / clock duration\n" " > 1 : faster than real time\n" " = 1 : real time\n" " < 1 : slower than real time\n", realTimeMultiplier); /* ASSERT(comp_time <= (finalTime-initialTime)); printf("testMuscles: PASSED Realtime test\n" " %s simulation time: %f with accuracy %f\n\n", actuatorType.c_str(), comp_time , accuracy); */ //An analysis only writes to a dir that exists, so create here. if (printResults == true){ Storage states(manager.getStateStorage()); states.print("testProbes_states.sto"); probeReporter->getProbeStorage().print("testProbes_probes.sto"); forceReporter->getForceStorage().print("testProbes_forces.sto"); muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto"); cout << "\nDone with printing results..." << endl; } double muscleWork = muscWorkProbe->getProbeOutputs(si)(0); cout << "Muscle work = " << muscleWork << endl; // Test the resetting of probes cout << "Resetting muscle work probe..." << endl; muscWorkProbe->reset(si); muscleWork = muscWorkProbe->getProbeOutputs(si)(0); cout << "Muscle work = " << muscleWork << endl; ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe."); //========================================================================== // 6. SIMULATION Tests //========================================================================== model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe."); ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe."); ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2 * pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe."); for (int i = 0; i<initCondVec.size(); ++i) { stringstream myError; //myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl; //ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str()); //if (testRealInitConditions(i) != initCondVec(i)) // cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl; } }
inline bool zsuper(STATE, CallFrame* call_frame, intptr_t literal) { Object* block = stack_pop(); Object* const recv = call_frame->self(); VariableScope* scope = call_frame->method_scope(state); interp_assert(scope); MachineCode* mc = scope->method()->machine_code(); Object* splat_obj = 0; Array* splat = 0; size_t arg_count = mc->total_args; if(mc->splat_position >= 0) { splat_obj = scope->get_local(state, mc->splat_position); splat = try_as<Array>(splat_obj); if(splat) { arg_count += splat->size(); } else { arg_count++; } } Tuple* tup = Tuple::create(state, arg_count); native_int tup_index = 0; native_int fixed_args; if(splat) { fixed_args = mc->splat_position; } else if(mc->keywords) { fixed_args = mc->total_args - 1; } else { fixed_args = mc->total_args; } for(native_int i = 0; i < fixed_args; i++) { tup->put(state, tup_index++, scope->get_local(state, i)); } if(splat) { for(native_int i = 0; i < splat->size(); i++) { tup->put(state, tup_index++, splat->get(state, i)); } } else if(splat_obj) { tup->put(state, tup_index++, splat_obj); } if(mc->post_args) { native_int post_position = mc->splat_position + 1; for(native_int i = post_position; i < post_position + mc->post_args; i++) { tup->put(state, tup_index++, scope->get_local(state, i)); } } if(mc->keywords) { native_int placeholder_position = splat_obj ? mc->total_args : mc->total_args - 1; native_int keywords_position = placeholder_position + 1; Object* placeholder = scope->get_local(state, placeholder_position); Array* ary = Array::create(state, 2); for(native_int i = keywords_position; i <= mc->keywords_count; i++) { ary->set(state, 0, as<Symbol>(call_frame->compiled_code->local_names()->at(state, i))); ary->set(state, 1, scope->get_local(state, i)); placeholder->send(state, state->symbol("[]="), ary); } tup->put(state, tup_index++, scope->get_local(state, placeholder_position)); } CallSite* call_site = reinterpret_cast<CallSite*>(literal); Arguments new_args(call_site->name(), recv, block, arg_count, 0); new_args.use_tuple(tup, arg_count); Object* ret; Symbol* current_name = call_frame->original_name(); if(call_site->name() != current_name) { call_site->name(state, current_name); } ret = call_site->execute(state, new_args); state->vm()->checkpoint(state); CHECK_AND_PUSH(ret); }
Array<T> fftconvolve(Array<T> const& signal, Array<T> const& filter, const bool expand, ConvolveBatchKind kind) { const af::dim4 sd = signal.dims(); const af::dim4 fd = filter.dims(); dim_t fftScale = 1; af::dim4 packed_dims; int fft_dims[baseDim]; af::dim4 sig_tmp_dims, sig_tmp_strides; af::dim4 filter_tmp_dims, filter_tmp_strides; // Pack both signal and filter on same memory array, this will ensure // better use of batched cuFFT capabilities for (dim_t k = 0; k < 4; k++) { if (k < baseDim) packed_dims[k] = nextpow2((unsigned)(sd[k] + fd[k] - 1)); else if (k == baseDim) packed_dims[k] = sd[k] + fd[k]; else packed_dims[k] = 1; if (k < baseDim) { fft_dims[baseDim-k-1] = (k == 0) ? packed_dims[k] / 2 : packed_dims[k]; fftScale *= fft_dims[baseDim-k-1]; } } Array<convT> packed = createEmptyArray<convT>(packed_dims); convT *packed_ptr = packed.get(); const af::dim4 packed_strides = packed.strides(); sig_tmp_dims[0] = filter_tmp_dims[0] = packed_dims[0]; sig_tmp_strides[0] = filter_tmp_strides[0] = 1; for (dim_t k = 1; k < 4; k++) { if (k < baseDim) { sig_tmp_dims[k] = packed_dims[k]; filter_tmp_dims[k] = packed_dims[k]; } else { sig_tmp_dims[k] = sd[k]; filter_tmp_dims[k] = fd[k]; } sig_tmp_strides[k] = sig_tmp_strides[k - 1] * sig_tmp_dims[k - 1]; filter_tmp_strides[k] = filter_tmp_strides[k - 1] * filter_tmp_dims[k - 1]; } // Calculate memory offsets for packed signal and filter convT *sig_tmp_ptr = packed_ptr; convT *filter_tmp_ptr = packed_ptr + sig_tmp_strides[3] * sig_tmp_dims[3]; // Number of packed complex elements in dimension 0 dim_t sig_half_d0 = divup(sd[0], 2); // Pack signal in a complex matrix where first dimension is half the input // (allows faster FFT computation) and pad array to a power of 2 with 0s packData<convT, T>(sig_tmp_ptr, sig_tmp_dims, sig_tmp_strides, signal); // Pad filter array with 0s padArray<convT, T>(filter_tmp_ptr, filter_tmp_dims, filter_tmp_strides, filter); // Compute forward FFT if (isDouble) { fftw_plan plan = fftw_plan_many_dft(baseDim, fft_dims, packed_dims[baseDim], (fftw_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, (fftw_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, FFTW_FORWARD, FFTW_ESTIMATE); fftw_execute(plan); fftw_destroy_plan(plan); } else { fftwf_plan plan = fftwf_plan_many_dft(baseDim, fft_dims, packed_dims[baseDim], (fftwf_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, (fftwf_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, FFTW_FORWARD, FFTW_ESTIMATE); fftwf_execute(plan); fftwf_destroy_plan(plan); } // Multiply filter and signal FFT arrays if (kind == ONE2MANY) complexMultiply<convT>(filter_tmp_ptr, filter_tmp_dims, filter_tmp_strides, sig_tmp_ptr, sig_tmp_dims, sig_tmp_strides, filter_tmp_ptr, filter_tmp_dims, filter_tmp_strides, kind); else complexMultiply<convT>(sig_tmp_ptr, sig_tmp_dims, sig_tmp_strides, sig_tmp_ptr, sig_tmp_dims, sig_tmp_strides, filter_tmp_ptr, filter_tmp_dims, filter_tmp_strides, kind); // Compute inverse FFT if (isDouble) { fftw_plan plan = fftw_plan_many_dft(baseDim, fft_dims, packed_dims[baseDim], (fftw_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, (fftw_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, FFTW_BACKWARD, FFTW_ESTIMATE); fftw_execute(plan); fftw_destroy_plan(plan); } else { fftwf_plan plan = fftwf_plan_many_dft(baseDim, fft_dims, packed_dims[baseDim], (fftwf_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, (fftwf_complex*)packed.get(), NULL, packed_strides[0], packed_strides[baseDim] / 2, FFTW_BACKWARD, FFTW_ESTIMATE); fftwf_execute(plan); fftwf_destroy_plan(plan); } // Compute output dimensions dim4 oDims(1); if (expand) { for(dim_t d=0; d<4; ++d) { if (kind==ONE2ONE || kind==ONE2MANY) { oDims[d] = sd[d]+fd[d]-1; } else { oDims[d] = (d<baseDim ? sd[d]+fd[d]-1 : sd[d]); } } } else { oDims = sd; if (kind==ONE2MANY) { for (dim_t i=baseDim; i<4; ++i) oDims[i] = fd[i]; } } Array<T> out = createEmptyArray<T>(oDims); T* out_ptr = out.get(); const af::dim4 out_dims = out.dims(); const af::dim4 out_strides = out.strides(); const af::dim4 filter_dims = filter.dims(); // Reorder the output if (kind == ONE2MANY) { reorderOutput<T, convT, roundOut> (out_ptr, out_dims, out_strides, filter_tmp_ptr, filter_tmp_dims, filter_tmp_strides, filter_dims, sig_half_d0, baseDim, fftScale, expand); } else { reorderOutput<T, convT, roundOut> (out_ptr, out_dims, out_strides, sig_tmp_ptr, sig_tmp_dims, sig_tmp_strides, filter_dims, sig_half_d0, baseDim, fftScale, expand); } return out; }
void SubImage::init (void) { tmpFT.zero(); copy(img); // copy current cut-out to (double) working copy. stats.getStats( img.get(), imgSize*imgSize, ST_VALUES ); double avg = stats.mean; size_t nEl = img.nElements(); double* imgPtr = img.get(); const double* winPtr = window.get(); if( object.fittedPlane.nElements() == nEl ) { const float* planePtr = object.fittedPlane.get(); for( size_t i=0; i<nEl; ++i) { // Note: mean(plane) = 0, so subtracting it will not affect the mean value. imgPtr[i] = (imgPtr[i]-avg-planePtr[i])*winPtr[i]+avg; } } else { transform(imgPtr, imgPtr+nEl, window.get(), imgPtr, [avg](const double& a, const double& b) { return (a-avg)*b+avg; } ); } stats.getStats( img.get(), imgSize*imgSize, ST_VALUES|ST_RMS ); // TODO test if this is necessary or if the stats for the larger area is sufficient string str = "Initializing image " + to_string(object.ID) + ":" + to_string(channel.ID) + ":" + to_string(index) + " mean=" + to_string (stats.mean) + " stddev=" + to_string (stats.stddev); transpose(img.get(),imgSize,imgSize); // to match MvN size_t noiseSize = noiseWindow.dimSize(0); if (imgSize == noiseSize) { img.copy(tmpImg); transform(tmpImg.get(), tmpImg.get()+tmpImg.nElements(), noiseWindow.get(), tmpImg.get(), [&](const double& a, const double& b) { return (a-stats.mean)*b; } ); imgFT.reset(tmpImg.get(), imgSize, imgSize, FT_FULLCOMPLEX); stats.noise = imgFT.noise(-1,-1); } else { size_t offset = (imgSize-noiseSize) / 2; Array<double> tmp(img, offset, offset+noiseSize-1, offset ,offset+noiseSize-1); tmp.trim(); transform(tmp.get(), tmp.get()+tmp.nElements(), noiseWindow.get(), tmp.get(), [&](const double& a, const double& b) { return (a-stats.mean)*b; } ); imgFT.reset(tmp.get(), imgSize, imgSize, FT_FULLCOMPLEX); stats.noise = imgFT.noise(-1,-1); } if (imgSize == otfSize) { // imgSize = 2*pupilSize imgFT.reset(img.get(), imgSize, imgSize, FT_FULLCOMPLEX); //|FT_NORMALIZE ); // full-complex for now, perhaps half-complex later for performance } else { // imgSize > 2*pupilSize should never happen (cf. calculatePupilSize) int offset = (otfSize - imgSize) / 2; Array<double> tmp (otfSize, otfSize); tmp.zero(); double* imgPtr = img.get(); double* tmpPtr = tmp.get(); for (int i = 0; i < imgSize; ++i) { memcpy (tmpPtr + offset * (otfSize + 1) + i * otfSize, imgPtr + i * imgSize, imgSize * sizeof (double)); } imgFT.reset (tmp.get(), otfSize, otfSize, FT_FULLCOMPLEX); //|FT_NORMALIZE ); // full-complex for now, perhaps half-complex later for performance } FourierTransform::reorder(imgFT); // keep FT in centered form stats.noise *= channel.noiseFudge; double rg = stats.noise/stats.stddev; str += " noise=" + to_string (stats.noise) + " rg=" + to_string(rg); object.addRegGamma( rg ); object.addToFT( imgFT ); LOG_DEBUG << str << " initial shift=" << (string)offsetShift << ende; }
void ExtendedException::computeBacktrace(bool skipFrame /* = false */) { Array bt = createBacktrace(BacktraceArgs() .skipTop(skipFrame) .withSelf()); m_btp = bt.get(); }
FatalErrorException::FatalErrorException(const std::string &msg, const Array& backtrace) { m_msg = msg; m_btp = backtrace.get(); }
Array<T> matmul(const Array<T> &lhs, const Array<T> &rhs, af_mat_prop optLhs, af_mat_prop optRhs) { #if defined(WITH_LINEAR_ALGEBRA) if(OpenCLCPUOffload(false)) { // Do not force offload gemm on OSX Intel devices return cpu::matmul(lhs, rhs, optLhs, optRhs); } #endif const auto lOpts = toBlasTranspose(optLhs); const auto rOpts = toBlasTranspose(optRhs); const auto aRowDim = (lOpts == OPENCL_BLAS_NO_TRANS) ? 0 : 1; const auto aColDim = (lOpts == OPENCL_BLAS_NO_TRANS) ? 1 : 0; const auto bColDim = (rOpts == OPENCL_BLAS_NO_TRANS) ? 1 : 0; const dim4 lDims = lhs.dims(); const dim4 rDims = rhs.dims(); const int M = lDims[aRowDim]; const int N = rDims[bColDim]; const int K = lDims[aColDim]; dim_t d2 = std::max(lDims[2], rDims[2]); dim_t d3 = std::max(lDims[3], rDims[3]); dim4 oDims = af::dim4(M, N, d2, d3); Array<T> out = createEmptyArray<T>(oDims); const auto alpha = scalar<T>(1); const auto beta = scalar<T>(0); const dim4 lStrides = lhs.strides(); const dim4 rStrides = rhs.strides(); const dim4 oStrides = out.strides(); int batchSize = oDims[2] * oDims[3]; bool is_l_d2_batched = oDims[2] == lDims[2]; bool is_l_d3_batched = oDims[3] == lDims[3]; bool is_r_d2_batched = oDims[2] == rDims[2]; bool is_r_d3_batched = oDims[3] == rDims[3]; for (int n = 0; n < batchSize; n++) { int w = n / oDims[2]; int z = n - w * oDims[2]; int loff = z * (is_l_d2_batched * lStrides[2]) + w * (is_l_d3_batched * lStrides[3]); int roff = z * (is_r_d2_batched * rStrides[2]) + w * (is_r_d3_batched * rStrides[3]); dim_t lOffset = lhs.getOffset() + loff; dim_t rOffset = rhs.getOffset() + roff; dim_t oOffset = out.getOffset() + z * oStrides[2] + w * oStrides[3]; cl::Event event; if(rDims[bColDim] == 1) { dim_t incr = (optRhs == AF_MAT_NONE) ? rStrides[0] : rStrides[1]; gpu_blas_gemv_func<T> gemv; OPENCL_BLAS_CHECK( gemv(lOpts, lDims[0], lDims[1], alpha, (*lhs.get())(), lOffset, lStrides[1], (*rhs.get())(), rOffset, incr, beta, (*out.get())(), oOffset, 1, 1, &getQueue()(), 0, nullptr, &event()) ); } else { gpu_blas_gemm_func<T> gemm; OPENCL_BLAS_CHECK( gemm(lOpts, rOpts, M, N, K, alpha, (*lhs.get())(), lOffset, lStrides[1], (*rhs.get())(), rOffset, rStrides[1], beta, (*out.get())(), oOffset, out.dims()[0], 1, &getQueue()(), 0, nullptr, &event()) ); } } return out; }
/* * Apply the wrap objects to the current path. */ void GeometryPath:: applyWrapObjects(const SimTK::State& s, Array<PathPoint*>& path) const { if (get_PathWrapSet().getSize() < 1) return; WrapResult best_wrap; Array<int> result, order; result.setSize(get_PathWrapSet().getSize()); order.setSize(get_PathWrapSet().getSize()); // Set the initial order to be the order they are listed in the path. for (int i = 0; i < get_PathWrapSet().getSize(); i++) order[i] = i; // If there is only one wrap object, calculate the wrapping only once. // If there are two or more objects, perform up to 8 iterations where // the result from one wrap object is used as the starting point for // the next wrap. const int maxIterations = get_PathWrapSet().getSize() < 2 ? 1 : 8; double last_length = SimTK::Infinity; for (int kk = 0; kk < maxIterations; kk++) { for (int i = 0; i < get_PathWrapSet().getSize(); i++) { result[i] = 0; PathWrap& ws = get_PathWrapSet().get(order[i]); const WrapObject* wo = ws.getWrapObject(); best_wrap.wrap_pts.setSize(0); double min_length_change = SimTK::Infinity; // First remove this object's wrapping points from the current path. for (int j = 0; j <path.getSize(); j++) { if( path.get(j) == &ws.getWrapPoint1()) { path.remove(j); // remove the first wrap point path.remove(j); // remove the second wrap point break; } } if (wo->get_active()) { // startPoint and endPoint in wrapStruct represent the // user-defined starting and ending points in the array of path // points that should be considered for wrapping. These indices // take into account via points, whether or not they are active. // Thus they are indices into mp_orig[], not mp[] (also, mp[] // may contain wrapping points from previous wrap objects, which // would mess up the starting and ending indices). But the goal // is to find starting and ending indices in mp[] to consider // for wrapping over this wrap object. Here is how that is done: // 1. startPoint and endPoint are 1-based, so subtract 1 from // them to get indices into get_PathPointSet(). -1 (or any value // less than 1) means use the first (or last) point. const int wrapStart = (ws.getStartPoint() < 1 ? 0 : ws.getStartPoint() - 1); const int wrapEnd = (ws.getEndPoint() < 1 ? get_PathPointSet().getSize() - 1 : ws.getEndPoint() - 1); // 2. Scan forward from wrapStart in get_PathPointSet() to find // the first point that is active. Store a pointer to it (smp). int jfwd = wrapStart; for (; jfwd <= wrapEnd; jfwd++) if (get_PathPointSet().get(jfwd).isActive(s)) break; if (jfwd > wrapEnd) // there are no active points in the path return; const PathPoint* const smp = &get_PathPointSet().get(jfwd); // 3. Scan backwards from wrapEnd in get_PathPointSet() to find // the last point that is active. Store a pointer to it (emp). int jrev = wrapEnd; for (; jrev >= wrapStart; jrev--) if (get_PathPointSet().get(jrev).isActive(s)) break; if (jrev < wrapStart) // there are no active points in the path return; const PathPoint* const emp = &get_PathPointSet().get(jrev); // 4. Now find the indices of smp and emp in _currentPath. int start=-1, end=-1; for (int j = 0; j < path.getSize(); j++) { if (path.get(j) == smp) start = j; if (path.get(j) == emp) end = j; } if (start == -1 || end == -1) // this should never happen return; // You now have indices into _currentPath (which is a list of // all currently active points, including wrap points) that // represent the used-defined range of points to consider for // wrapping over this wrap object. Check each path segment in // this range, choosing the best wrap as the one that changes // the path segment length the least: for (int pt1 = start; pt1 < end; pt1++) { const int pt2 = pt1 + 1; // As long as the two points are not auto wrap points on the // same wrap object, check them for wrapping. if ( path.get(pt1)->getWrapObject() == NULL || path.get(pt2)->getWrapObject() == NULL || ( path.get(pt1)->getWrapObject() != path.get(pt2)->getWrapObject())) { WrapResult wr; wr.startPoint = pt1; wr.endPoint = pt2; result[i] = wo->wrapPathSegment(s, *path.get(pt1), *path.get(pt2), ws, wr); if (result[i] == WrapObject::mandatoryWrap) { // "mandatoryWrap" means the path actually // intersected the wrap object. In this case, you // *must* choose this segment as the "best" one for // wrapping. If the path has more than one segment // that intersects the object, the first one is // taken as the mandatory wrap (this is considered // an ill-conditioned case). best_wrap = wr; // Store the best wrap in the pathWrap for possible // use next time. ws.setPreviousWrap(wr); break; } else if (result[i] == WrapObject::wrapped) { // "wrapped" means the path segment was wrapped over // the object, but you should consider the other // segments as well to see if one // wraps with a smaller length change. double path_length_change = calcPathLengthChange(s, *wo, wr, path); if (path_length_change < min_length_change) { best_wrap = wr; // Store the best wrap in the pathWrap for // possible use next time ws.setPreviousWrap(wr); min_length_change = path_length_change; } else { // The wrap was not shorter than the current // minimum, so just free the wrap points that // were allocated. wr.wrap_pts.setSize(0); } } else { // Nothing to do. } } } // Deallocate previous wrapping points if necessary. ws.updWrapPoint2().getWrapPath().setSize(0); if (best_wrap.wrap_pts.getSize() == 0) { ws.resetPreviousWrap(); ws.updWrapPoint2().getWrapPath().setSize(0); } else { // If wrapping did occur, copy wrap info into the PathStruct. ws.updWrapPoint1().getWrapPath().setSize(0); Array<SimTK::Vec3>& wrapPath = ws.updWrapPoint2().getWrapPath(); wrapPath = best_wrap.wrap_pts; // In OpenSim, all conversion to/from the wrap object's // reference frame will be performed inside // wrapPathSegment(). Thus, all points in this function will // be in their respective body reference frames. // for (j = 0; j < wrapPath.getSize(); j++){ // convert_from_wrap_object_frame(wo, wrapPath.get(j)); // convert(ms->modelnum, wrapPath.get(j), wo->segment, // ms->ground_segment); // } ws.updWrapPoint1().setWrapLength(0.0); ws.updWrapPoint2().setWrapLength(best_wrap.wrap_path_length); ws.updWrapPoint1().setLocation(s,best_wrap.r1); ws.updWrapPoint2().setLocation(s,best_wrap.r2); // Now insert the two new wrapping points into mp[] array. path.insert(best_wrap.endPoint, &ws.updWrapPoint1()); path.insert(best_wrap.endPoint + 1, &ws.updWrapPoint2()); } } } const double length = calcLengthAfterPathComputation(s, path); if (std::abs(length - last_length) < 0.0005) { break; } else { last_length = length; } if (kk == 0 && get_PathWrapSet().getSize() > 1) { // If the first wrap was a no wrap, and the second was a no wrap // because a point was inside the object, switch the order of // the first two objects and try again. if ( result[0] == WrapObject::noWrap && result[1] == WrapObject::insideRadius) { order[0] = 1; order[1] = 0; // remove wrap object 0 from the list of path points PathWrap& ws = get_PathWrapSet().get(0); for (int j = 0; j < path.getSize(); j++) { if (path.get(j) == &ws.updWrapPoint1()) { path.remove(j); // remove the first wrap point path.remove(j); // remove the second wrap point break; } } } } } }
void writeDeviceDataArray(Array<T> &arr, const void *const data, const size_t bytes) { if (!arr.isOwner()) { arr = copyArray<T>(arr); } memcpy(arr.get(), (const T *const)data, bytes); }
/* Look at this class and it's superclass contents (which includes * included modules) and calculate out how to allocate the slots. * * This locks the class so that construction is serialized. */ void Class::auto_pack(STATE, GCToken gct, CallFrame* call_frame) { Class* self = this; OnStack<1> os(state, self); hard_lock(state, gct, call_frame); // If another thread did this work while we were waiting on the lock, // don't redo it. if(self->type_info_->type == PackedObject::type) { self->hard_unlock(state, gct, call_frame); return; } size_t slots = 0; LookupTable* lt = LookupTable::create(state); // If autopacking is enabled, figure out how many slots to use. if(state->shared().config.gc_autopack) { Module* mod = self; int slot = 0; while(!mod->nil_p()) { Array* info = 0; if(Class* cls = try_as<Class>(mod)) { info = cls->seen_ivars(); } else if(IncludedModule* im = try_as<IncludedModule>(mod)) { info = im->module()->seen_ivars(); } if(info && !info->nil_p()) { for(native_int i = 0; i < info->size(); i++) { if(Symbol* sym = try_as<Symbol>(info->get(state, i))) { bool found = false; lt->fetch(state, sym, &found); if(!found) { lt->store(state, sym, Fixnum::from(slot++)); } } // Limit the number of packed ivars to 25. if(slot > 25) break; } } mod = mod->superclass(); } slots = lt->entries()->to_native(); } self->set_packed_size(sizeof(Object) + (slots * sizeof(Object*))); self->packed_ivar_info(state, lt); atomic::memory_barrier(); self->set_object_type(state, PackedObject::type); self->hard_unlock(state, gct, call_frame); }
unsigned orb(Array<float> &x, Array<float> &y, Array<float> &score, Array<float> &ori, Array<float> &size, Array<uint> &desc, const Array<T>& image, const float fast_thr, const unsigned max_feat, const float scl_fctr, const unsigned levels, const bool blur_img) { unsigned patch_size = REF_PAT_SIZE; const af::dim4 idims = image.dims(); unsigned min_side = std::min(idims[0], idims[1]); unsigned max_levels = 0; float scl_sum = 0.f; for (unsigned i = 0; i < levels; i++) { min_side /= scl_fctr; // Minimum image side for a descriptor to be computed if (min_side < patch_size || max_levels == levels) break; max_levels++; scl_sum += 1.f / (float)std::pow(scl_fctr,(float)i); } std::vector<float*> h_x_pyr(max_levels); std::vector<float*> h_y_pyr(max_levels); std::vector<float*> h_score_pyr(max_levels); std::vector<float*> h_ori_pyr(max_levels); std::vector<float*> h_size_pyr(max_levels); std::vector<unsigned*> h_desc_pyr(max_levels); std::vector<unsigned> feat_pyr(max_levels); unsigned total_feat = 0; // Compute number of features to keep for each level std::vector<unsigned> lvl_best(max_levels); unsigned feat_sum = 0; for (unsigned i = 0; i < max_levels-1; i++) { float lvl_scl = (float)std::pow(scl_fctr,(float)i); lvl_best[i] = ceil((max_feat / scl_sum) / lvl_scl); feat_sum += lvl_best[i]; } lvl_best[max_levels-1] = max_feat - feat_sum; // Maintain a reference to previous level image Array<T> prev_img = createEmptyArray<T>(af::dim4()); af::dim4 prev_ldims; af::dim4 gauss_dims(9); T* h_gauss = nullptr; Array<T> gauss_filter = createEmptyArray<T>(af::dim4()); for (unsigned i = 0; i < max_levels; i++) { af::dim4 ldims; const float lvl_scl = (float)std::pow(scl_fctr,(float)i); Array<T> lvl_img = createEmptyArray<T>(af::dim4()); if (i == 0) { // First level is used in its original size lvl_img = image; ldims = image.dims(); prev_img = image; prev_ldims = image.dims(); } else { // Resize previous level image to current level dimensions ldims[0] = round(idims[0] / lvl_scl); ldims[1] = round(idims[1] / lvl_scl); lvl_img = resize<T>(prev_img, ldims[0], ldims[1], AF_INTERP_BILINEAR); prev_img = lvl_img; prev_ldims = lvl_img.dims(); } Array<float> x_feat = createEmptyArray<float>(dim4()); Array<float> y_feat = createEmptyArray<float>(dim4()); Array<float> score_feat = createEmptyArray<float>(dim4()); // Round feature size to nearest odd integer float size = 2.f * floor(patch_size / 2.f) + 1.f; // Avoid keeping features that might be too wide and might not fit on // the image, sqrt(2.f) is the radius when angle is 45 degrees and // represents widest case possible unsigned edge = ceil(size * sqrt(2.f) / 2.f); unsigned lvl_feat = fast(x_feat, y_feat, score_feat, lvl_img, fast_thr, 9, 1, 0.15f, edge); if (lvl_feat == 0) { continue; } float* h_x_feat = x_feat.get(); float* h_y_feat = y_feat.get(); float* h_x_harris = memAlloc<float>(lvl_feat); float* h_y_harris = memAlloc<float>(lvl_feat); float* h_score_harris = memAlloc<float>(lvl_feat); // Calculate Harris responses // Good block_size >= 7 (must be an odd number) unsigned usable_feat = 0; harris_response<T, false>(h_x_harris, h_y_harris, h_score_harris, nullptr, h_x_feat, h_y_feat, nullptr, lvl_feat, &usable_feat, lvl_img, 7, 0.04f, patch_size); if (usable_feat == 0) { memFree(h_x_harris); memFree(h_y_harris); memFree(h_score_harris); continue; } // Sort features according to Harris responses af::dim4 usable_feat_dims(usable_feat); Array<float> score_harris = createHostDataArray(usable_feat_dims, h_score_harris); Array<float> harris_sorted = createEmptyArray<float>(af::dim4()); Array<unsigned> harris_idx = createEmptyArray<unsigned>(af::dim4()); sort_index<float, false>(harris_sorted, harris_idx, score_harris, 0); memFree(h_score_harris); usable_feat = std::min(usable_feat, lvl_best[i]); if (usable_feat == 0) { memFree(h_x_harris); memFree(h_y_harris); continue; } float* h_x_lvl = memAlloc<float>(usable_feat); float* h_y_lvl = memAlloc<float>(usable_feat); float* h_score_lvl = memAlloc<float>(usable_feat); // Keep only features with higher Harris responses keep_features<T>(h_x_lvl, h_y_lvl, h_score_lvl, nullptr, h_x_harris, h_y_harris, harris_sorted.get(), harris_idx.get(), nullptr, usable_feat); memFree(h_x_harris); memFree(h_y_harris); float* h_ori_lvl = memAlloc<float>(usable_feat); float* h_size_lvl = memAlloc<float>(usable_feat); // Compute orientation of features centroid_angle<T>(h_x_lvl, h_y_lvl, h_ori_lvl, usable_feat, lvl_img, patch_size); Array<T> lvl_filt = createEmptyArray<T>(dim4()); if (blur_img) { // Calculate a separable Gaussian kernel, if one is not already stored if (!h_gauss) { h_gauss = memAlloc<T>(gauss_dims[0]); gaussian1D(h_gauss, gauss_dims[0], 2.f); gauss_filter = createHostDataArray(gauss_dims, h_gauss); } // Filter level image with Gaussian kernel to reduce noise sensitivity lvl_filt = convolve2<T, convAccT, false>(lvl_img, gauss_filter, gauss_filter); } // Compute ORB descriptors unsigned* h_desc_lvl = memAlloc<unsigned>(usable_feat * 8); memset(h_desc_lvl, 0, usable_feat * 8 * sizeof(unsigned)); if (blur_img) extract_orb<T>(h_desc_lvl, usable_feat, h_x_lvl, h_y_lvl, h_ori_lvl, h_size_lvl, lvl_filt, lvl_scl, patch_size); else extract_orb<T>(h_desc_lvl, usable_feat, h_x_lvl, h_y_lvl, h_ori_lvl, h_size_lvl, lvl_img, lvl_scl, patch_size); // Store results to pyramids total_feat += usable_feat; feat_pyr[i] = usable_feat; h_x_pyr[i] = h_x_lvl; h_y_pyr[i] = h_y_lvl; h_score_pyr[i] = h_score_lvl; h_ori_pyr[i] = h_ori_lvl; h_size_pyr[i] = h_size_lvl; h_desc_pyr[i] = h_desc_lvl; } if (h_gauss != nullptr) memFree(h_gauss); if (total_feat > 0 ) { // Allocate feature Arrays const af::dim4 total_feat_dims(total_feat); const af::dim4 desc_dims(8, total_feat); x = createEmptyArray<float>(total_feat_dims); y = createEmptyArray<float>(total_feat_dims); score = createEmptyArray<float>(total_feat_dims); ori = createEmptyArray<float>(total_feat_dims); size = createEmptyArray<float>(total_feat_dims); desc = createEmptyArray<uint >(desc_dims); float* h_x = x.get(); float* h_y = y.get(); float* h_score = score.get(); float* h_ori = ori.get(); float* h_size = size.get(); unsigned* h_desc = desc.get(); unsigned offset = 0; for (unsigned i = 0; i < max_levels; i++) { if (feat_pyr[i] == 0) continue; if (i > 0) offset += feat_pyr[i-1]; memcpy(h_x+offset, h_x_pyr[i], feat_pyr[i] * sizeof(float)); memcpy(h_y+offset, h_y_pyr[i], feat_pyr[i] * sizeof(float)); memcpy(h_score+offset, h_score_pyr[i], feat_pyr[i] * sizeof(float)); memcpy(h_ori+offset, h_ori_pyr[i], feat_pyr[i] * sizeof(float)); memcpy(h_size+offset, h_size_pyr[i], feat_pyr[i] * sizeof(float)); memcpy(h_desc+(offset*8), h_desc_pyr[i], feat_pyr[i] * 8 * sizeof(unsigned)); memFree(h_x_pyr[i]); memFree(h_y_pyr[i]); memFree(h_score_pyr[i]); memFree(h_ori_pyr[i]); memFree(h_size_pyr[i]); memFree(h_desc_pyr[i]); } } return total_feat; }
void harris_response( float* x_out, float* y_out, float* score_out, float* size_out, const float* x_in, const float* y_in, const float* scl_in, const unsigned total_feat, unsigned* usable_feat, const Array<T>& image, const unsigned block_size, const float k_thr, const unsigned patch_size) { const af::dim4 idims = image.dims(); const T* image_ptr = image.get(); for (unsigned f = 0; f < total_feat; f++) { unsigned x, y; float scl = 1.f; if (use_scl) { // Update x and y coordinates according to scale scl = scl_in[f]; x = (unsigned)round(x_in[f] * scl); y = (unsigned)round(y_in[f] * scl); } else { x = (unsigned)round(x_in[f]); y = (unsigned)round(y_in[f]); } // Round feature size to nearest odd integer float size = 2.f * floor((patch_size * scl) / 2.f) + 1.f; // Avoid keeping features that might be too wide and might not fit on // the image, sqrt(2.f) is the radius when angle is 45 degrees and // represents widest case possible unsigned patch_r = ceil(size * sqrt(2.f) / 2.f); if (x < patch_r || y < patch_r || x >= idims[1] - patch_r || y >= idims[0] - patch_r) continue; unsigned r = block_size / 2; float ixx = 0.f, iyy = 0.f, ixy = 0.f; unsigned block_size_sq = block_size * block_size; for (unsigned k = 0; k < block_size_sq; k++) { int i = k / block_size - r; int j = k % block_size - r; // Calculate local x and y derivatives float ix = image_ptr[(x+i+1) * idims[0] + y+j] - image_ptr[(x+i-1) * idims[0] + y+j]; float iy = image_ptr[(x+i) * idims[0] + y+j+1] - image_ptr[(x+i) * idims[0] + y+j-1]; // Accumulate second order derivatives ixx += ix*ix; iyy += iy*iy; ixy += ix*iy; } unsigned idx = *usable_feat; *usable_feat += 1; float tr = ixx + iyy; float det = ixx*iyy - ixy*ixy; // Calculate Harris responses float resp = det - k_thr * (tr*tr); // Scale factor // TODO: improve response scaling float rscale = 0.001f; rscale = rscale * rscale * rscale * rscale; x_out[idx] = x; y_out[idx] = y; score_out[idx] = resp * rscale; if (use_scl) size_out[idx] = size; } }
bool EngineTimerHandler::received(Message &msg) { String query; query = "SELECT registro as enabled, protocol, usuario as username, interval, codecs as formats, usuario as authname, senha as password, enderecoip || ':' || porta as server, domain, outbound , localaddress, modified, enderecoip as account, id as gateway_id, status, TRUE AS gw FROM provedores WHERE registro IS TRUE AND modified IS TRUE AND usuario IS NOT NULL"; // Output("Query:'%s'",query.c_str()); Message *m = new Message("database"); m->setParam("account","ipbx"); m->setParam("query",query); m->setParam("results",String::boolText(true)); if (Engine::dispatch(m)) { int rows = m->getIntValue("rows"); if (rows>0) { int j; for (int i=1 ; i<=rows ; i++) { Message *msg= new Message("user.login"); Array* a = static_cast<Array*>(m->userObject("Array")); for (j = 0; j < a->getColumns(); j++) { String* s = YOBJECT(String,a->get(j,0)); if (!(s && *s)) continue; String name = *s; s = YOBJECT(String,a->get(j,i)); if (!s) continue; msg->setParam(name,*s); Output("Retornou: '%s' = '%s'",name.c_str(), s->c_str()); } Engine::enqueue(msg); } } } query = "UPDATE ramais SET location=NULL,expires=NULL WHERE expires IS NOT NULL AND expires<=CURRENT_TIMESTAMP; UPDATE provedores SET modified='f' WHERE modified='t' AND usuario IS NOT NULL"; // Output("Query:'%s'",query.c_str()); m = new Message("database"); m->setParam("account","ipbx"); m->setParam("query",query); m->setParam("results",String::boolText(false)); if (!Engine::dispatch(m)) { Output("Error Dispatch!!!"); return false; } return true; }
xdebug_xml_node* xdebug_var_export_xml_node(const char* name, const char* fullName, const char* facet, const Variant& var, XDebugExporter& exporter) { // Setup the node. Each const cast is necessary due to xml api xdebug_xml_node* node = xdebug_xml_node_init("property"); if (name != nullptr) { xdebug_xml_add_attribute_ex(node, "name", const_cast<char*>(name), 0, 1); } if (fullName != nullptr) { xdebug_xml_add_attribute_ex(node, "fullname", const_cast<char*>(fullName), 0, 1); } if (facet != nullptr) { xdebug_xml_add_attribute_ex(node, "facet", const_cast<char*>(facet), 0, 1); } xdebug_xml_add_attribute_ex(node, "address", xdebug_sprintf("%ld", (long) &var), 0, 1); // Case on the type for the rest if (var.isBoolean()) { xdebug_xml_add_attribute(node, "type", "bool"); xdebug_xml_add_text(node, xdebug_sprintf("%d", var.toBoolean())); } else if (var.isNull()) { xdebug_xml_add_attribute(node, "type", "null"); } else if (var.isInteger()) { xdebug_xml_add_attribute(node, "type", "int"); xdebug_xml_add_text(node, xdebug_sprintf("%ld", var.toInt64())); } else if (var.isDouble()) { xdebug_xml_add_attribute(node, "type", "float"); xdebug_xml_add_text(node, xdebug_sprintf("%lG", var.toDouble())); } else if (var.isString()) { // Add the type and the original size String str = var.toString(); xdebug_xml_add_attribute(node, "type", "string"); xdebug_xml_add_attribute_ex(node, "size", xdebug_sprintf("%d", str.size()), 0, 1); // Possibly shrink the string, then add it to the node if (exporter.max_data != 0 && str.size() > exporter.max_data) { str = str.substr(0, exporter.max_data); } xdebug_xml_add_text_encodel(node, xdstrdup(str.data()), str.size()); } else if (var.isArray()) { Array arr = var.toArray(); xdebug_xml_add_attribute(node, "type", "array"); xdebug_xml_add_attribute(node, "children", const_cast<char*>(arr.size() > 0 ? "1" : "0")); // If we've already seen this object, return if (exporter.counts[arr.get()]++ > 0) { xdebug_xml_add_attribute(node, "recursive", "1"); return node; } // Write the # of children then short-circuit if we are too deep xdebug_xml_add_attribute_ex(node, "numchildren", xdebug_sprintf("%d", arr.size()), 0, 1); if (exporter.level++ >= exporter.max_depth) { return node; } // Compute the page and the start/end indices // Note that php xdebug doesn't support pages except for at the top level uint32_t page = exporter.level == 0 ? exporter.page : 0; uint32_t start = page * exporter.max_children; uint32_t end = (page + 1) * exporter.max_children; xdebug_xml_add_attribute_ex(node, "page", xdebug_sprintf("%d", page), 0, 1); xdebug_xml_add_attribute_ex(node, "pagesize", xdebug_sprintf("%d", exporter.max_children), 0, 1); // Add each child ArrayIter iter(arr); iter.setPos(start); for (uint32_t i = start; i < end && iter; i++, ++iter) { xdebug_array_element_export_xml_node(*node, name, iter.first(), iter.second(), exporter); } // Done at this level exporter.level--; exporter.counts[arr.get()]--; } else if (var.isObject()) { // TODO(#4489053) This could be merged into the above array code. For now, // it's separate as this was pulled originally from xdebug ObjectData* obj = var.toObject().get(); Class* cls = obj->getVMClass(); Array props = get_object_props(obj); // Add object info xdebug_xml_add_attribute(node, "type", "object"); xdebug_xml_add_attribute_ex(node, "classname", xdstrdup(cls->name()->data()), 0, 1); xdebug_xml_add_attribute(node, "children", const_cast<char*>(props.size() ? "1" : "0")); // If we've already seen this object, return if (exporter.counts[obj]++ > 0) { return node; } // Add the # of props then short circuit if we are too deep xdebug_xml_add_attribute_ex(node, "numchildren", xdebug_sprintf("%d", props.size()), 0, 1); if (exporter.level++ >= exporter.max_depth) { return node; } // Compute the page and the start/end indices // Note that php xdebug doesn't support pages except for at the top level uint32_t page = exporter.level == 1 ? exporter.page : 0; uint32_t start = page * exporter.max_children; uint32_t end = (page + 1) * exporter.max_children; xdebug_xml_add_attribute_ex(node, "page", xdebug_sprintf("%d", page), 0, 1); xdebug_xml_add_attribute_ex(node, "pagesize", xdebug_sprintf("%d", exporter.max_children), 0, 1); // Add each property ArrayIter iter(props); iter.setPos(start); for (uint32_t i = start; i < end && iter; i++, ++iter) { xdebug_object_element_export_xml_node(*node, name, obj, iter.first(), iter.second(), exporter); } // Done at this level exporter.level--; exporter.counts[(void*) obj]--; } else if (var.isResource()) { ResourceData* res = var.toResource().get(); xdebug_xml_add_attribute(node, "type", "resource"); const char* text = xdebug_sprintf("resource id='%ld' type='%s'", res->o_getId(), res->o_getResourceName().data()); xdebug_xml_add_text(node, const_cast<char*>(text)); } else { xdebug_xml_add_attribute(node, "type", "null"); } return node; }
Error encode_variant(const Variant &p_variant, uint8_t *r_buffer, int &r_len, bool p_object_as_id) { uint8_t *buf = r_buffer; r_len = 0; uint32_t flags = 0; switch (p_variant.get_type()) { case Variant::INT: { int64_t val = p_variant; if (val > 0x7FFFFFFF || val < -0x80000000) { flags |= ENCODE_FLAG_64; } } break; case Variant::REAL: { double d = p_variant; float f = d; if (double(f) != d) { flags |= ENCODE_FLAG_64; //always encode real as double } } break; case Variant::OBJECT: { if (p_object_as_id) { flags |= ENCODE_FLAG_OBJECT_AS_ID; } } break; } if (buf) { encode_uint32(p_variant.get_type() | flags, buf); buf += 4; } r_len += 4; switch (p_variant.get_type()) { case Variant::NIL: { //nothing to do } break; case Variant::BOOL: { if (buf) { encode_uint32(p_variant.operator bool(), buf); } r_len += 4; } break; case Variant::INT: { int64_t val = p_variant; if (val > 0x7FFFFFFF || val < -0x80000000) { //64 bits if (buf) { encode_uint64(val, buf); } r_len += 8; } else { if (buf) { encode_uint32(int32_t(val), buf); } r_len += 4; } } break; case Variant::REAL: { double d = p_variant; float f = d; if (double(f) != d) { if (buf) { encode_double(p_variant.operator double(), buf); } r_len += 8; } else { if (buf) { encode_float(p_variant.operator float(), buf); } r_len += 4; } } break; case Variant::NODE_PATH: { NodePath np = p_variant; if (buf) { encode_uint32(uint32_t(np.get_name_count()) | 0x80000000, buf); //for compatibility with the old format encode_uint32(np.get_subname_count(), buf + 4); uint32_t flags = 0; if (np.is_absolute()) flags |= 1; if (np.get_property() != StringName()) flags |= 2; encode_uint32(flags, buf + 8); buf += 12; } r_len += 12; int total = np.get_name_count() + np.get_subname_count(); if (np.get_property() != StringName()) total++; for (int i = 0; i < total; i++) { String str; if (i < np.get_name_count()) str = np.get_name(i); else if (i < np.get_name_count() + np.get_subname_count()) str = np.get_subname(i - np.get_subname_count()); else str = np.get_property(); CharString utf8 = str.utf8(); int pad = 0; if (utf8.length() % 4) pad = 4 - utf8.length() % 4; if (buf) { encode_uint32(utf8.length(), buf); buf += 4; copymem(buf, utf8.get_data(), utf8.length()); buf += pad + utf8.length(); } r_len += 4 + utf8.length() + pad; } } break; case Variant::STRING: { _encode_string(p_variant, buf, r_len); } break; // math types case Variant::VECTOR2: { if (buf) { Vector2 v2 = p_variant; encode_float(v2.x, &buf[0]); encode_float(v2.y, &buf[4]); } r_len += 2 * 4; } break; // 5 case Variant::RECT2: { if (buf) { Rect2 r2 = p_variant; encode_float(r2.position.x, &buf[0]); encode_float(r2.position.y, &buf[4]); encode_float(r2.size.x, &buf[8]); encode_float(r2.size.y, &buf[12]); } r_len += 4 * 4; } break; case Variant::VECTOR3: { if (buf) { Vector3 v3 = p_variant; encode_float(v3.x, &buf[0]); encode_float(v3.y, &buf[4]); encode_float(v3.z, &buf[8]); } r_len += 3 * 4; } break; case Variant::TRANSFORM2D: { if (buf) { Transform2D val = p_variant; for (int i = 0; i < 3; i++) { for (int j = 0; j < 2; j++) { copymem(&buf[(i * 2 + j) * 4], &val.elements[i][j], sizeof(float)); } } } r_len += 6 * 4; } break; case Variant::PLANE: { if (buf) { Plane p = p_variant; encode_float(p.normal.x, &buf[0]); encode_float(p.normal.y, &buf[4]); encode_float(p.normal.z, &buf[8]); encode_float(p.d, &buf[12]); } r_len += 4 * 4; } break; case Variant::QUAT: { if (buf) { Quat q = p_variant; encode_float(q.x, &buf[0]); encode_float(q.y, &buf[4]); encode_float(q.z, &buf[8]); encode_float(q.w, &buf[12]); } r_len += 4 * 4; } break; case Variant::RECT3: { if (buf) { Rect3 aabb = p_variant; encode_float(aabb.position.x, &buf[0]); encode_float(aabb.position.y, &buf[4]); encode_float(aabb.position.z, &buf[8]); encode_float(aabb.size.x, &buf[12]); encode_float(aabb.size.y, &buf[16]); encode_float(aabb.size.z, &buf[20]); } r_len += 6 * 4; } break; case Variant::BASIS: { if (buf) { Basis val = p_variant; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { copymem(&buf[(i * 3 + j) * 4], &val.elements[i][j], sizeof(float)); } } } r_len += 9 * 4; } break; case Variant::TRANSFORM: { if (buf) { Transform val = p_variant; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { copymem(&buf[(i * 3 + j) * 4], &val.basis.elements[i][j], sizeof(float)); } } encode_float(val.origin.x, &buf[36]); encode_float(val.origin.y, &buf[40]); encode_float(val.origin.z, &buf[44]); } r_len += 12 * 4; } break; // misc types case Variant::COLOR: { if (buf) { Color c = p_variant; encode_float(c.r, &buf[0]); encode_float(c.g, &buf[4]); encode_float(c.b, &buf[8]); encode_float(c.a, &buf[12]); } r_len += 4 * 4; } break; /*case Variant::RESOURCE: { ERR_EXPLAIN("Can't marshallize resources"); ERR_FAIL_V(ERR_INVALID_DATA); //no, i'm sorry, no go } break;*/ case Variant::_RID: { } break; case Variant::OBJECT: { if (p_object_as_id) { if (buf) { Object *obj = p_variant; ObjectID id = 0; if (obj && ObjectDB::instance_validate(obj)) { id = obj->get_instance_id(); } encode_uint64(id, buf); } r_len += 8; } else { Object *obj = p_variant; if (!obj) { if (buf) { encode_uint32(0, buf); buf += 4; r_len += 4; } } else { _encode_string(obj->get_class(), buf, r_len); List<PropertyInfo> props; obj->get_property_list(&props); int pc = 0; for (List<PropertyInfo>::Element *E = props.front(); E; E = E->next()) { if (!(E->get().usage & PROPERTY_USAGE_STORAGE)) continue; pc++; } if (buf) { encode_uint32(pc, buf); buf += 4; } r_len += 4; for (List<PropertyInfo>::Element *E = props.front(); E; E = E->next()) { if (!(E->get().usage & PROPERTY_USAGE_STORAGE)) continue; _encode_string(E->get().name, buf, r_len); int len; Error err = encode_variant(obj->get(E->get().name), buf, len, p_object_as_id); if (err) return err; ERR_FAIL_COND_V(len % 4, ERR_BUG); r_len += len; if (buf) buf += len; } } } } break; case Variant::DICTIONARY: { Dictionary d = p_variant; if (buf) { encode_uint32(uint32_t(d.size()), buf); buf += 4; } r_len += 4; List<Variant> keys; d.get_key_list(&keys); for (List<Variant>::Element *E = keys.front(); E; E = E->next()) { /* CharString utf8 = E->->utf8(); if (buf) { encode_uint32(utf8.length()+1,buf); buf+=4; copymem(buf,utf8.get_data(),utf8.length()+1); } r_len+=4+utf8.length()+1; while (r_len%4) r_len++; //pad */ int len; encode_variant(E->get(), buf, len, p_object_as_id); ERR_FAIL_COND_V(len % 4, ERR_BUG); r_len += len; if (buf) buf += len; encode_variant(d[E->get()], buf, len, p_object_as_id); ERR_FAIL_COND_V(len % 4, ERR_BUG); r_len += len; if (buf) buf += len; } } break; case Variant::ARRAY: { Array v = p_variant; if (buf) { encode_uint32(uint32_t(v.size()), buf); buf += 4; } r_len += 4; for (int i = 0; i < v.size(); i++) { int len; encode_variant(v.get(i), buf, len, p_object_as_id); ERR_FAIL_COND_V(len % 4, ERR_BUG); r_len += len; if (buf) buf += len; } } break; // arrays case Variant::POOL_BYTE_ARRAY: { PoolVector<uint8_t> data = p_variant; int datalen = data.size(); int datasize = sizeof(uint8_t); if (buf) { encode_uint32(datalen, buf); buf += 4; PoolVector<uint8_t>::Read r = data.read(); copymem(buf, &r[0], datalen * datasize); } r_len += 4 + datalen * datasize; while (r_len % 4) r_len++; } break; case Variant::POOL_INT_ARRAY: { PoolVector<int> data = p_variant; int datalen = data.size(); int datasize = sizeof(int32_t); if (buf) { encode_uint32(datalen, buf); buf += 4; PoolVector<int>::Read r = data.read(); for (int i = 0; i < datalen; i++) encode_uint32(r[i], &buf[i * datasize]); } r_len += 4 + datalen * datasize; } break; case Variant::POOL_REAL_ARRAY: { PoolVector<real_t> data = p_variant; int datalen = data.size(); int datasize = sizeof(real_t); if (buf) { encode_uint32(datalen, buf); buf += 4; PoolVector<real_t>::Read r = data.read(); for (int i = 0; i < datalen; i++) encode_float(r[i], &buf[i * datasize]); } r_len += 4 + datalen * datasize; } break; case Variant::POOL_STRING_ARRAY: { PoolVector<String> data = p_variant; int len = data.size(); if (buf) { encode_uint32(len, buf); buf += 4; } r_len += 4; for (int i = 0; i < len; i++) { CharString utf8 = data.get(i).utf8(); if (buf) { encode_uint32(utf8.length() + 1, buf); buf += 4; copymem(buf, utf8.get_data(), utf8.length() + 1); buf += utf8.length() + 1; } r_len += 4 + utf8.length() + 1; while (r_len % 4) { r_len++; //pad if (buf) buf++; } } } break; case Variant::POOL_VECTOR2_ARRAY: { PoolVector<Vector2> data = p_variant; int len = data.size(); if (buf) { encode_uint32(len, buf); buf += 4; } r_len += 4; if (buf) { for (int i = 0; i < len; i++) { Vector2 v = data.get(i); encode_float(v.x, &buf[0]); encode_float(v.y, &buf[4]); buf += 4 * 2; } } r_len += 4 * 2 * len; } break; case Variant::POOL_VECTOR3_ARRAY: { PoolVector<Vector3> data = p_variant; int len = data.size(); if (buf) { encode_uint32(len, buf); buf += 4; } r_len += 4; if (buf) { for (int i = 0; i < len; i++) { Vector3 v = data.get(i); encode_float(v.x, &buf[0]); encode_float(v.y, &buf[4]); encode_float(v.z, &buf[8]); buf += 4 * 3; } } r_len += 4 * 3 * len; } break; case Variant::POOL_COLOR_ARRAY: { PoolVector<Color> data = p_variant; int len = data.size(); if (buf) { encode_uint32(len, buf); buf += 4; } r_len += 4; if (buf) { for (int i = 0; i < len; i++) { Color c = data.get(i); encode_float(c.r, &buf[0]); encode_float(c.g, &buf[4]); encode_float(c.b, &buf[8]); encode_float(c.a, &buf[12]); buf += 4 * 4; } } r_len += 4 * 4 * len; } break; default: { ERR_FAIL_V(ERR_BUG); } } return OK; }
Fracture* VertPositionMutation::mutate(Fracture* fracture) { // get all verts that are not corners Array<Vertex*>* nonCorner = new Array<Vertex*>(); for(int i=0;i<fracture->getVerts()->getSize();i++) if(!fracture->getVerts()->get(i)->getIsCorner()) nonCorner->add(fracture->getVerts()->get(i)); int numVerts = nonCorner->getSize(); if(!numVerts) // if only corners return the original fracture return fracture; // get a random non-boundary vert int randVert = RNG::RandomInt(numVerts); Vertex* ranVert = nonCorner->get(randVert); // get the move amount and distance before making a jump real moveLimit = EvolutionSettings::getInstance()->getMaxMovePercent(); real distBeforeJump = EvolutionSettings::getInstance()->getDistBeforeJump(); if(ranVert->getBoundary()) { // move along boundary line Array<Edge*>* boundaryEdges = new Array<Edge*>(); for(int i=0;i<ranVert->getEdges()->getSize();i++) if(ranVert->getEdges()->get(i)->getIsBoundary()) boundaryEdges->add(ranVert->getEdges()->get(i)); // choose a random edge to move by int randDir = RNG::RandomInt(boundaryEdges->getSize()); Edge* edgeToMoveBy = boundaryEdges->get(randDir); // get the edge length real length = edgeToMoveBy->length(); // if the length is too small move the other direction // in the future this is going to check if it can jump i.e // it will jump around a corner. if(length < distBeforeJump) { for(int i=0;i<boundaryEdges->getSize();i++) { // this should always work since there should be at least // two boundary edges attached if(boundaryEdges->get(i)!=edgeToMoveBy) { edgeToMoveBy = boundaryEdges->get(i); i = boundaryEdges->getSize(); } } // recheck length and abort if the check fails real length = edgeToMoveBy->length(); if(length < distBeforeJump) return fracture; } // get a random mutation value real mutateScale = RNG::RandomFloat(moveLimit); // Get the two points on the edge to move on Point2 firstPoint = ranVert->getLocation(); Point2 secondPoint = edgeToMoveBy->getOtherPoint(ranVert->getID()); // calculate the vert's new location Point2 slope = secondPoint.minus(firstPoint); slope.scale(mutateScale); Point2 newLoc = firstPoint.add(slope); // set the new location ranVert->setLocation(newLoc); // tell the verts edges about its new location ranVert->updateEdges(); // clean up while(boundaryEdges->getSize()) boundaryEdges->removeLast(); delete boundaryEdges; } else { // maybe implement move along edges ??? // move about faces using barycentric coordinates // get the faces containing the point Array<Face*>* facesWithVert = fracture->getFacesWithVertex(ranVert); // create a face containing all points around the vert Face* faceToMutateAround = new Face(); for(int i=0;i<facesWithVert->getSize();i++) { Face* tmp = facesWithVert->get(i); for(int j=0;j<tmp->getEdges()->getSize();j++) if(!tmp->getEdges()->get(j)->eitherMatch(ranVert->getID())) faceToMutateAround->getEdges()->add(tmp->getEdges()->get(j)->copy()); for(int j=0;j<tmp->getVerts()->getSize();j++) if(tmp->getVerts()->get(j)->getID() != ranVert->getID()) faceToMutateAround->getVerts()->add(tmp->getVerts()->get(j)->copy(faceToMutateAround->getEdges())); } // create the container for the generated Tris Array<Tri*>* generatedTris = new Array<Tri*>(); // detect if the face is convex or not faceToMutateAround->detectIfConvex(); if(faceToMutateAround->getIsConvex()) { // TODO :: Do this the more efficient way // convex case mutation (easy) // generate tris for(int i=0;i<faceToMutateAround->getEdges()->getSize();i++) generatedTris->add(new Tri(faceToMutateAround->getEdges()->get(i),ranVert->getLocation(),ranVert->getID())); // create barycentric coordinates for mutation (2 * #tris) // TODO :: Combine this with the case below int numBarys = generatedTris->getSize()*2; real barys[numBarys]; for(int i=0;i<numBarys;i++) barys[i] = RNG::RandomFloat(moveLimit); // apply mutations Point2 newPos; newPos.xpos = 0.0; newPos.ypos = 0.0; int pointID = ranVert->getID(); for(int i=0;i<generatedTris->getSize();i++) { // get new position real baryOne = barys[i*2]; real baryTwo = barys[i*2-1]; real baryThree = 1.0 - baryOne - baryTwo; newPos = generatedTris->get(i)->interpolatePosition(baryOne,baryTwo,baryThree); // update the position in all the remaining tris for(int j=i;j<generatedTris->getSize();j++) generatedTris->get(j)->updatePosition(pointID,newPos); } // set new position of vert ranVert->setLocation(newPos); // update edges ranVert->updateEdges(); // clean up } else { // concave case mutation (harder) // create collection for trimesh shell Array<Vertex*>* vertsInView = new Array<Vertex*>(); Array<Vertex*>* sortedVertsInView = new Array<Vertex*>(); Array<Edge*>* trimeshShell = new Array<Edge*>(); // get all verts in view for(int i=0;i<faceToMutateAround->getVerts()->getSize();i++) { Vertex* vert = faceToMutateAround->getVerts()->get(i); // create a temp edge to check for intersections Edge* tmpEdge = new Edge(ranVert->getLocation(),vert->getLocation()); bool noIntersection = true; // check if intersections for(int i=0;i<faceToMutateAround->getEdges()->getSize();i++) if(faceToMutateAround->getEdges()->get(i)->intersects(tmpEdge)) noIntersection = false; // if no intersections add it to the list of verts in view if(noIntersection) vertsInView->add(vert); // Note :: vert is still a copy // clean up delete tmpEdge; } // sort verts Array<Integer>* sortedIDs = faceToMutateAround->sortVertIDsByPath(); for(int i=0;i<sortedIDs->getSize();i++) for(int j=0;j<vertsInView->getSize();i++) if(vertsInView->get(j)->getID() == sortedIDs->get(i).val) { sortedVertsInView->add(vertsInView->get(j)); j = vertsInView->getSize(); } // create shell from verts for(int i=0;i<sortedVertsInView->getSize();i++) { Vertex* one = sortedVertsInView->get(i); Vertex* two = i==sortedVertsInView->getSize()-1 ? sortedVertsInView->get(0) : sortedVertsInView->get(i+1); Edge* newEdge = new Edge(one->getLocation(),two->getLocation(),one->getID(),two->getID()); trimeshShell->add(newEdge); } // generate tris for(int i=0;i<trimeshShell->getSize();i++) generatedTris->add(new Tri(trimeshShell->get(i),ranVert->getLocation(),ranVert->getID())); // create barycentric coordinates for mutation (Only 2 for this case) // TODO :: Combine this with the case above //int numBarys = generatedTris->getSize()*2; int numBarys = 2; real barys[numBarys]; for(int i=0;i<numBarys;i++) barys[i] = RNG::RandomFloat(moveLimit); // apply mutations Point2 newPos; newPos.xpos = 0.0; newPos.ypos = 0.0; int pointID = ranVert->getID(); int ranTri = RNG::RandomInt(generatedTris->getSize()); //for(int i=0;i<generatedTris->getSize();i++) { // get new position //real baryOne = barys[i*2]; //real baryTwo = barys[i*2-1]; real baryOne = barys[0]; real baryTwo = barys[1]; real baryThree = 1.0 - baryOne - baryTwo; newPos = generatedTris->get(ranTri)->interpolatePosition(baryOne,baryTwo,baryThree); // update the position in all the remaining tris (skip for this step) // for(int j=i;j<generatedTris->getSize();j++) // generatedTris->get(j)->updatePosition(pointID,newPos); //} // set new position of vert ranVert->setLocation(newPos); // update edges ranVert->updateEdges(); // clean up while(vertsInView->getSize()) vertsInView->removeLast(); while(sortedIDs->getSize()) sortedIDs->removeLast(); while(sortedVertsInView->getSize()) sortedVertsInView->removeLast(); while(trimeshShell->getSize()) delete trimeshShell->removeLast(); delete vertsInView; delete trimeshShell; delete sortedVertsInView; delete sortedIDs; } // clean up while(nonCorner->getSize()) nonCorner->removeLast(); while(facesWithVert->getSize()) facesWithVert->removeLast(); while(generatedTris->getSize()) delete generatedTris->removeLast(); while(faceToMutateAround->getVerts()->getSize()) delete faceToMutateAround->getVerts()->removeLast(); while(faceToMutateAround->getEdges()->getSize()) delete faceToMutateAround->getEdges()->removeLast(); delete faceToMutateAround; delete generatedTris; delete facesWithVert; delete nonCorner; } return fracture; }
void clfft_common(Array<T> &arr) { const dim4 dims = arr.dims(); const dim4 strides = arr.strides(); size_t io_strides[]= {(size_t)strides[0], (size_t)strides[1], (size_t)strides[2], (size_t)strides[3]}; size_t rank_dims[3] = {(size_t)dims[0], (size_t)dims[1], (size_t)dims[2]}; clfftPlanHandle plan; find_clfft_plan(plan, (clfftDim)rank, rank_dims, io_strides, io_strides[rank], io_strides, io_strides[rank], (clfftPrecision)Precision<T>::type, dims[rank]); CLFFT_ERROR_CHECK( clfftEnqueueTransform(plan, direction, 1, &(getQueue()()), 0, NULL, NULL, &((*arr.get())()), NULL, NULL) ); }
void test_aset() { Array* ary = Array::create(state, 2); ary->aset(state, Fixnum::from(1), Fixnum::from(4)); TS_ASSERT_EQUALS(4, as<Fixnum>(ary->get(state, 1))->to_native()); }
Array<T> copyArray(const Array<T> &A) { Array<T> out = createEmptyArray<T>(A.dims()); copyData(out.get(), A); return out; }
FatalErrorException::FatalErrorException(const std::string& msg, const Array& backtrace, bool isRecoverable /* = false */) : ExtendedException(msg, backtrace.get()), m_recoverable(isRecoverable) {}
Array<T> * meanshift(const Array<T> &in, const float &s_sigma, const float &c_sigma, const unsigned iter) { const dim4 dims = in.dims(); const dim4 istrides = in.strides(); Array<T>* out = createEmptyArray<T>(dims); const dim4 ostrides = out->strides(); const dim_type bIndex = (is_color ? 3 : 2); const dim_type bCount = dims[bIndex]; const dim_type channels = (is_color ? dims[2] : 1); // clamp spatical and chromatic sigma's float space_ = std::min(11.5f, s_sigma); const dim_type radius = std::max((dim_type)(space_ * 1.5f), 1); const float cvar = c_sigma*c_sigma; vector<float> means(channels); vector<float> centers(channels); vector<float> tmpclrs(channels); for(dim_type batchId=0; batchId<bCount; ++batchId) { T *outData = out->get() + batchId*ostrides[bIndex]; const T * inData = in.get() + batchId*istrides[bIndex]; for(dim_type j=0; j<dims[1]; ++j) { dim_type j_in_off = j*istrides[1]; dim_type j_out_off = j*ostrides[1]; for(dim_type i=0; i<dims[0]; ++i) { dim_type i_in_off = i*istrides[0]; dim_type i_out_off = i*ostrides[0]; // clear means and centers for this pixel for(dim_type ch=0; ch<channels; ++ch) { means[ch] = 0.0f; // the expression ch*istrides[2] will only effect when ch>1 // i.e for color images where batch is along fourth dimension centers[ch] = inData[j_in_off + i_in_off + ch*istrides[2]]; } // scope of meanshift iterationd begin for(unsigned it=0; it<iter; ++it) { int count = 0; int shift_x = 0; int shift_y = 0; for(dim_type wj=-radius; wj<=radius; ++wj) { int hit_count = 0; for(dim_type wi=-radius; wi<=radius; ++wi) { dim_type tj = j + wj; dim_type ti = i + wi; // clamps offsets tj = clamp(tj, 0ll, dims[1]-1); ti = clamp(ti, 0ll, dims[0]-1); // proceed float norm = 0.0f; for(dim_type ch=0; ch<channels; ++ch) { tmpclrs[ch] = inData[ tj*istrides[1] + ti*istrides[0] + ch*istrides[2]]; norm += (centers[ch]-tmpclrs[ch]) * (centers[ch]-tmpclrs[ch]); } if (norm<= cvar) { for(dim_type ch=0; ch<channels; ++ch) means[ch] += tmpclrs[ch]; shift_x += wi; ++hit_count; } } count+= hit_count; shift_y += wj*hit_count; } if (count==0) { break; } const float fcount = 1.f/count; const int mean_x = (int)(shift_x*fcount+0.5f); const int mean_y = (int)(shift_y*fcount+0.5f); for(dim_type ch=0; ch<channels; ++ch) means[ch] *= fcount; float norm = 0.f; for(dim_type ch=0; ch<channels; ++ch) norm += ((means[ch]-centers[ch])*(means[ch]-centers[ch])); bool stop = ((abs(shift_y-mean_y)+abs(shift_x-mean_x)) + norm) <= 1; shift_x = mean_x; shift_y = mean_y; for(dim_type ch=0; ch<channels; ++ch) centers[ch] = means[ch]; if (stop) { break; } } // scope of meanshift iterations end for(dim_type ch=0; ch<channels; ++ch) outData[j_out_off + i_out_off + ch*ostrides[2]] = centers[ch]; } } } return out; }