/*==============================================================================
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;
}
