nsresult
MediaEngineGonkVideoSource::Allocate(const dom::MediaTrackConstraints& aConstraints,
const MediaEnginePrefs& aPrefs,
const nsString& aDeviceId,
const nsACString& aOrigin,
AllocationHandle** aOutHandle,
const char** aOutBadConstraint)
{
LOG((__FUNCTION__));
ReentrantMonitorAutoEnter sync(mCallbackMonitor);
if (mState == kReleased && mInitDone) {
NormalizedConstraints constraints(aConstraints);
ChooseCapability(constraints, aPrefs, aDeviceId);
NS_DispatchToMainThread(WrapRunnable(RefPtr<MediaEngineGonkVideoSource>(this),
&MediaEngineGonkVideoSource::AllocImpl));
mCallbackMonitor.Wait();
if (mState != kAllocated) {
return NS_ERROR_FAILURE;
}
}
*aOutHandle = nullptr;
return NS_OK;
}
NOX::Abstract::Group::ReturnType
LOCA::MultiContinuation::CompositeConstraint::computeConstraints()
{
if (isValidConstraints)
return NOX::Abstract::Group::Ok;
std::string callingFunction =
"LOCA::MultiContinuation::CompositeConstraint::computeConstraints()";
NOX::Abstract::Group::ReturnType status;
NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;
const NOX::Abstract::MultiVector::DenseMatrix *g;
for (int i=0; i<numConstraintObjects; i++) {
status = constraintPtrs[i]->computeConstraints();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
g = &(constraintPtrs[i]->getConstraints());
for (int j=0; j<constraintPtrs[i]->numConstraints(); j++)
constraints(indices[i][j],0) = (*g)(j,0);
}
isValidConstraints = true;
return finalStatus;
}
constraint mk_class_instance_cnstr(std::shared_ptr<class_instance_context> const & C, local_context const & ctx, expr const & m, unsigned depth) {
environment const & env = C->env();
justification j = mk_failed_to_synthesize_jst(env, m);
auto choice_fn = [=](expr const & meta, expr const & meta_type, substitution const &, name_generator const &) {
if (auto cls_name_it = is_ext_class(C->tc(), meta_type)) {
name cls_name = *cls_name_it;
list<expr> const & ctx_lst = ctx.get_data();
list<expr> local_insts;
if (C->use_local_instances())
local_insts = get_local_instances(C->tc(), ctx_lst, cls_name);
list<name> insts = get_class_instances(env, cls_name);
if (empty(local_insts) && empty(insts))
return lazy_list<constraints>(); // nothing to be done
// we are always strict with placeholders associated with classes
return choose(std::make_shared<class_instance_elaborator>(C, ctx, meta, meta_type, local_insts, insts, j, depth));
} else {
// do nothing, type is not a class...
return lazy_list<constraints>(constraints());
}
};
bool owner = false;
bool relax = C->m_relax;
return mk_choice_cnstr(m, choice_fn, to_delay_factor(cnstr_group::Basic),
owner, j, relax);
}
bool TopologyConstraints::
assertFeasible() const {
vector<TopologyConstraint*> ts;
constraints(ts);
for_each(ts.begin(),ts.end(),mem_fun(&TopologyConstraint::assertFeasible));
return true;
}
NOX::Abstract::Group::ReturnType
LOCA::MultiContinuation::MultiVecConstraint::computeDP(
const std::vector<int>& paramIDs,
NOX::Abstract::MultiVector::DenseMatrix& dgdp,
bool isValidG)
{
std::string callingFunction =
"LOCA::MultiContinuation::MultiVecConstraint::computeDP()";
NOX::Abstract::Group::ReturnType status;
// Compute constraints if necessary
if (!isValidG && !isValidConstraints)
status = computeConstraints();
if (!isValidG) {
for (int i=0; i<constraints.numRows(); i++)
dgdp(i,0) = constraints(i,0);
}
// Set rest of dgdp to zero
for (unsigned int j=0; j<paramIDs.size(); j++)
for (int i=0; i<constraints.numRows(); i++)
dgdp(i,j+1) = 0.0;
return status;
}
NOX::Abstract::Group::ReturnType
LOCA::TurningPoint::MinimallyAugmented::Constraint::
computeDP(const std::vector<int>& paramIDs,
NOX::Abstract::MultiVector::DenseMatrix& dgdp,
bool isValidG)
{
std::string callingFunction =
"LOCA::TurningPoint::MinimallyAugmented::Constraint::computeDP()";
NOX::Abstract::Group::ReturnType status;
NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;
// Compute sigma, w and v if necessary
if (!isValidConstraints) {
status = computeConstraints();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
}
// Compute -(w^T*J*v)_p
status = grpPtr->computeDwtJnDp(paramIDs, (*w_vector)[0], (*v_vector)[0],
dgdp, false);
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
dgdp.scale(-1.0/sigma_scale);
// Set the first column of dgdp
dgdp(0,0) = constraints(0,0);
return finalStatus;
}
int ProblemInstance::objectiveVariableTypes(int objective, int* real, int* binary, int* integer) const
{
unsigned offset = 4*(constraints()+objective);
*real = m_variableCounts[offset+1];
*binary = m_variableCounts[offset+2];
*integer = m_variableCounts[offset+3];
return m_variableCounts[offset];
}
NOX::Abstract::Group::ReturnType
NormConstraint::computeConstraints()
{
constraints(0,0) = 0.5 * n * (*x)[0].innerProduct((*x)[0]) - p.getValue("Constraint Param");
isValidConstraints = true;
return NOX::Abstract::Group::Ok;
}
NOX::Abstract::Group::ReturnType
LOCA::Hopf::MinimallyAugmented::Constraint::
computeDP(const vector<int>& paramIDs,
NOX::Abstract::MultiVector::DenseMatrix& dgdp,
bool isValidG)
{
string callingFunction =
"LOCA::Hopf::MinimallyAugmented::Constraint::computeDP()";
NOX::Abstract::Group::ReturnType status;
NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;
// Compute sigma, w and v if necessary
if (!isValidConstraints) {
status = computeConstraints();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
}
// Compute -(w^T*J*v)_p
NOX::Abstract::MultiVector::DenseMatrix dgdp_real(Teuchos::View, dgdp,
1, paramIDs.size()+1,
0, 0);
NOX::Abstract::MultiVector::DenseMatrix dgdp_imag(Teuchos::View, dgdp,
1, paramIDs.size()+1,
1, 0);
status = grpPtr->computeDwtCeDp(paramIDs,
(*w_vector)[0], (*w_vector)[1],
(*v_vector)[0], (*v_vector)[1],
omega,
dgdp_real, dgdp_imag, false);
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
dgdp.scale(-1.0/sigma_scale);
// Set the first column of dgdp
dgdp(0,0) = constraints(0,0);
dgdp(1,0) = constraints(1,0);
return finalStatus;
}
// input : degree
void stablize_mode(float ef_desire_roll, float ef_desire_pitch, float bf_desire_yaw_rate, float hover){
float d_q[4], e_q[4];
float bf_desire_rate[3];
float norm, angle_error;
float phi = ef_desire_roll*0.008726646; // degree to radian
float theta = ef_desire_pitch*0.008726646;
float c_phi = cosf(phi); float s_phi = sinf(phi);
float c_theta = cosf(theta); float s_theta = sinf(theta);
d_q[0] = c_phi*c_theta;
d_q[1] = s_phi*c_theta;
d_q[2] = c_phi*s_theta;
d_q[3] = -s_phi*s_theta;
e_q[0] = d_q[0]*_quaternion[0] + d_q[1]*_quaternion[1] + d_q[2]*_quaternion[2] + d_q[3]*_quaternion[3];
e_q[1] = d_q[1]*_quaternion[0] - d_q[0]*_quaternion[1] + d_q[3]*_quaternion[2] - d_q[2]*_quaternion[3];
e_q[2] = d_q[2]*_quaternion[0] - d_q[3]*_quaternion[1] - d_q[0]*_quaternion[2] + d_q[1]*_quaternion[3];
e_q[3] = d_q[3]*_quaternion[0] + d_q[2]*_quaternion[1] - d_q[1]*_quaternion[2] - d_q[0]*_quaternion[3];
norm = sqrtf(e_q[0]*e_q[0]+e_q[1]*e_q[1]+e_q[2]*e_q[2]+e_q[3]*e_q[3]);
e_q[0] /= norm;
if(e_q[0] < 0){
e_q[0] *= -1;
e_q[1] *= -1;
e_q[2] *= -1;
e_q[3] *= -1;
}
angle_error = acosf(e_q[0])*180.0/3.14159;
if(_prev_angle_error - angle_error > 3){
angle_error = _prev_angle_error - 3;
}else if(angle_error - _prev_angle_error > 3){
angle_error = _prev_angle_error + 3;
}
_prev_angle_error = angle_error;
norm = sqrtf(e_q[1]*e_q[1]+e_q[2]*e_q[2]+e_q[3]*e_q[3]);
bf_desire_rate[0] = e_q[1]/norm;
bf_desire_rate[1] = e_q[2]/norm;
bf_desire_rate[2] = e_q[3]/norm;
angle_error = stable_Kp*angle_error*2; // Kp*2*angle*180/3.14
angle_error = constraints(angle_error,0.0, 45.0); // max 20deg/s
bf_desire_rate[0] *= angle_error;
bf_desire_rate[1] *= angle_error;
//bf_desire_rate[2] *= angle_error;
bf_desire_rate[2] *= bf_desire_yaw_rate;
rate_controller(bf_desire_rate, 0, hover);
}
// input : deg/s
// rate_type : 1-> earth frame desire rate(roll, pitch, yaw)
// rate_type : 0-> body frame desire rate(p,q,r)
void rate_controller(float* desire_rate, unsigned long rate_type, float hover){
_r_rate_PID.myInput = _w_gyro[0]*57.29577951; // deg/s
_p_rate_PID.myInput = _w_gyro[1]*57.29577951;
_y_rate_PID.myInput = _w_gyro[2]*57.29577951;
if(rate_type == 1){
// earth frame to body frame
_r_rate_PID.mySetpoint = desire_rate[0] - sinf(_euler_angle[1])*desire_rate[2];
_p_rate_PID.mySetpoint = cosf(_euler_angle[0])*desire_rate[1] + sinf(_euler_angle[0])*cosf(_euler_angle[1])*desire_rate[2];
_y_rate_PID.mySetpoint = -sinf(_euler_angle[1])*desire_rate[1] + cosf(_euler_angle[0])*sinf(_euler_angle[1])*desire_rate[2];
}else{
_r_rate_PID.mySetpoint = desire_rate[0];
_p_rate_PID.mySetpoint = desire_rate[1];
_y_rate_PID.mySetpoint = desire_rate[2];
}
PID_Compute(&_r_rate_PID);
PID_Compute(&_p_rate_PID);
PID_Compute(&_y_rate_PID);
_r_rate_out = _r_rate_PID.myOutput;
_p_rate_out = _p_rate_PID.myOutput;
_y_rate_out = _y_rate_PID.myOutput;
_front_out = hover*front_factor - _p_rate_out - _y_rate_out;
_right_out = hover*right_factor - _r_rate_out + _y_rate_out;
_back_out = hover*back_factor + _p_rate_out - _y_rate_out;
_left_out = hover*left_factor + _r_rate_out + _y_rate_out;
_front_out = constraints(_front_out,7, 60);
_right_out = constraints(_right_out,7, 60);
_back_out = constraints(_back_out,7, 60);
_left_out = constraints(_left_out,7, 60);
Motor_write(front_motor, _front_out);
Motor_write(right_motor, _right_out);
Motor_write(back_motor, _back_out);
Motor_write(left_motor, _left_out);
}
bool TopologyConstraints::solve() {
FILE_LOG(logDEBUG)<<"TopologyConstraints::solve... dim="<<dim;
COLA_ASSERT(assertConvexBends(edges));
COLA_ASSERT(assertNoSegmentRectIntersection(nodes,edges));
COLA_ASSERT(assertFeasible());
vector<TopologyConstraint*> ts;
constraints(ts);
vpsc::IncSolver s(vs,cs);
s.solve();
double minTAlpha=1;
TopologyConstraint* minT=nullptr;
//printEdges(edges);
// find minimum feasible alpha over all topology constraints
for(vector<TopologyConstraint*>::iterator i=ts.begin();
i!=ts.end();++i) {
TopologyConstraint* t=*i;
FILE_LOG(logDEBUG1)<<"Checking topology constraint:"<<t->toString();
double tAlpha=t->c->maxSafeAlpha();
FILE_LOG(logDEBUG1)<<" alpha="<<tAlpha;
if(tAlpha<minTAlpha) {
minTAlpha=tAlpha;
minT=t;
}
}
#ifndef NDEBUG
if(minT) {
FILE_LOG(logDEBUG)<<" minT="<<minT->toString();
FILE_LOG(logDEBUG)<<" minTAlpha="<<minTAlpha;
} else {
FILE_LOG(logDEBUG)<<" No violated constraints!";
}
#endif
if(minTAlpha>0) {
for(Nodes::iterator i=nodes.begin();i!=nodes.end();++i) {
Node* v=*i;
v->rect->moveCentreD(dim,v->posOnLine(dim, minTAlpha));
}
}
COLA_ASSERT(noOverlaps());
COLA_ASSERT(assertConvexBends(edges));
// rectangle and edge point positions updated to variables.
FILE_LOG(logDEBUG)<<" moves done.";
if(minTAlpha<1 && minT) {
// now we satisfy the violated topology constraint, i.e. a bend point
// that has become straight is removed or a segment that needs to bend
// is split
minT->satisfy();
}
//printEdges(edges);
COLA_ASSERT(assertFeasible());
COLA_ASSERT(assertConvexBends(edges));
COLA_ASSERT(assertNoSegmentRectIntersection(nodes,edges));
FILE_LOG(logDEBUG)<<"TopologyConstraints::solve... done";
return minT!=nullptr;
}
int main(int argc, char **argv)
{
// Vérifier le nombre d'arguments
if (argc != 3)
{
std::cerr << "Utilisation : " << argv[0] << " <contraintes> <sortie>" << std::endl;
return 1;
}
std::string binary_name(argv[0]);
std::string constraints(argv[1]);
std::string output(argv[2]);
int question_number = 3;
if (binary_name.size() >= 13)
{
char c = binary_name[binary_name.size()-5];
if (c == '1') question_number = 1;
else if (c == '2') question_number = 2;
else if (c == '3') question_number = 3;
}
// Parser le fichier de contraintes
Parser1 *parser;
// Résoudre le problème
Problem1 *problem;
if(question_number == 1)
{
parser = new Parser1(constraints);
problem = new Problem1(parser);
}
else if(question_number == 2)
{
parser = new Parser2(constraints);
problem = new Problem2(parser);
}
else if(question_number == 3)
{
parser = new Parser3(constraints);
problem = new Problem3(parser);
}
problem->addAllClauses();
problem->solve();
problem->printResult();
problem->write(output);
delete problem;
delete parser;
}
lazy_list<constraints> choose(std::shared_ptr<choice_iterator> c, bool ignore_failure) {
return mk_lazy_list<constraints>([=]() {
auto s = c->next();
if (s) {
return some(mk_pair(*s, choose(c, false)));
} else if (ignore_failure) {
// return singleton empty list of constraints, and let tactic hints try to instantiate the metavariable.
return lazy_list<constraints>::maybe_pair(constraints(), lazy_list<constraints>());
} else {
return lazy_list<constraints>::maybe_pair();
}
});
}
nsresult
MediaEngineWebRTCMicrophoneSource::Restart(AllocationHandle* aHandle,
const dom::MediaTrackConstraints& aConstraints,
const MediaEnginePrefs &aPrefs,
const nsString& aDeviceId,
const char** aOutBadConstraint)
{
AssertIsOnOwningThread();
MOZ_ASSERT(aHandle);
NormalizedConstraints constraints(aConstraints);
return ReevaluateAllocation(aHandle, &constraints, aPrefs, aDeviceId,
aOutBadConstraint);
}
NOX::Abstract::Group::ReturnType
LOCA::MultiContinuation::ArcLengthConstraint::computeDP(
const vector<int>& paramIDs,
NOX::Abstract::MultiVector::DenseMatrix& dgdp,
bool isValidG)
{
string callingFunction =
"LOCA::MultiContinuation::ArcLengthConstraint::computeDP()";
NOX::Abstract::Group::ReturnType status;
NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;
// Compute constraints if necessary
if (!isValidG && !isValidConstraints) {
status = computeConstraints();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
}
if (!isValidG) {
for (int i=0; i<constraints.numRows(); i++)
dgdp(i,0) = constraints(i,0);
}
// Get tangent vector
const LOCA::MultiContinuation::ExtendedMultiVector& scaledTangent =
arcLengthGroup->getScaledPredictorTangent();
// If a param ID is equal to a constraint param ID, then that column
// of dgdp is given by that column of the scaled predictor, other wise
// that column is zero
vector<int>::const_iterator it;
int idx;
for (unsigned int i=0; i<paramIDs.size(); i++) {
it = find(conParamIDs.begin(), conParamIDs.end(), paramIDs[i]);
if (it == conParamIDs.end())
for (int k=0; k<constraints.numRows(); k++)
dgdp(k,i+1) = 0.0;
else {
idx = it - conParamIDs.begin();
for (int k=0; k<constraints.numRows(); k++)
dgdp(k,i+1) = scaledTangent.getScalar(k,idx);
}
}
return finalStatus;
}
nsresult
MediaEngineRemoteVideoSource::Restart(AllocationHandle* aHandle,
const dom::MediaTrackConstraints& aConstraints,
const MediaEnginePrefs& aPrefs,
const nsString& aDeviceId,
const char** aOutBadConstraint)
{
AssertIsOnOwningThread();
if (!mInitDone) {
LOG(("Init not done"));
return NS_ERROR_FAILURE;
}
MOZ_ASSERT(aHandle);
NormalizedConstraints constraints(aConstraints);
return ReevaluateAllocation(aHandle, &constraints, aPrefs, aDeviceId,
aOutBadConstraint);
}
Rcpp::List el_sem_euclid_weights(SEXP b_weights_r, SEXP y_r, SEXP omega_r, SEXP b_r)
{
//matrix which holds the observed data
mat y = as<arma::mat>(y_r);
int n = y.n_cols; //number of observations
int v = y.n_rows; //number of variables
//find appropriate spots to put in b_weights_r
uvec b_spots = find(as<arma::mat>(b_r)); //non-structural zeros in B
mat b_weights(v, v, fill::zeros); // matrix with edge weights
b_weights.elem(b_spots) = as<arma::vec>(b_weights_r);
uvec gamma_indices = arma::find(trimatu(as<arma::mat>(omega_r) == 0 )); //structural 0's in Omega
mat constraints(v + gamma_indices.n_elem, n); //constraints containing mean and covariance restrictions
constraints.rows(0, v - 1) = (eye(v, v) - b_weights) * y ; //mean restrictions
//covariance restrictions
int i,j,k;
for(k = 0; k < gamma_indices.n_elem; k++) {
j = (int) gamma_indices(k) / v;
i = (int) gamma_indices(k) % v;
constraints.row(k + v) = constraints.row(i) % constraints.row(j);
}
vec avg_constraints = mean(constraints, 1);
mat S(v + gamma_indices.n_elem, v + gamma_indices.n_elem, fill::zeros);
for(i = 0; i < n; i++){
S += constraints.col(i) * (avg_constraints.t() - constraints.col(i).t() );
}
S = S / n;
vec dual = solve(S, avg_constraints);
constraints.each_col() -= avg_constraints;
vec p_star = (1.0/ n) * (1 + (dual.t() * constraints).t());
double objective = - 1.0/2 * sum(pow(n * p_star - 1.0 ,2));
return Rcpp::List::create(Rcpp::Named("p_star", p_star),
Rcpp::Named("objective", objective));
}
NOX::Abstract::Group::ReturnType
LOCA::Pitchfork::MinimallyAugmented::Constraint::
computeConstraints()
{
if (isValidConstraints)
return NOX::Abstract::Group::Ok;
// Compute sigma
NOX::Abstract::Group::ReturnType status =
LOCA::TurningPoint::MinimallyAugmented::Constraint::computeConstraints();
pf_constraints(0,0) = constraints(0,0);
// Compute <psi,x>
pf_constraints(1,0) = pf_grp->innerProduct(*psi_vector, pf_grp->getX());
return status;
}
tactic apply_tactic_core(elaborate_fn const & elab, expr const & e, add_meta_kind add_meta, subgoals_action_kind k) {
return tactic([=](environment const & env, io_state const & ios, proof_state const & s) {
goals const & gs = s.get_goals();
if (empty(gs)) {
throw_no_goal_if_enabled(s);
return proof_state_seq();
}
goal const & g = head(gs);
name_generator ngen = s.get_ngen();
expr new_e; substitution new_subst; constraints cs_;
auto ecs = elab(g, ngen.mk_child(), e, none_expr(), s.get_subst(), false);
std::tie(new_e, new_subst, cs_) = ecs;
buffer<constraint> cs;
to_buffer(cs_, cs);
to_buffer(s.get_postponed(), cs);
proof_state new_s(s, new_subst, ngen, constraints());
return apply_tactic_core(env, ios, new_s, new_e, cs, add_meta, k);
});
}
NOX::Abstract::Group::ReturnType
LOCA::MultiContinuation::ArcLengthConstraint::computeConstraints()
{
if (isValidConstraints)
return NOX::Abstract::Group::Ok;
string callingFunction =
"LOCA::MultiContinuation::ArcLengthConstraint::computeConstraints()";
NOX::Abstract::Group::ReturnType status;
NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;
// Compute predictor if necessary
if (!arcLengthGroup->isPredictor()) {
status = arcLengthGroup->computePredictor();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
}
// Get tangent vector
const LOCA::MultiContinuation::ExtendedMultiVector& scaledTangent =
arcLengthGroup->getScaledPredictorTangent();
const LOCA::MultiContinuation::ExtendedMultiVector& tangent =
arcLengthGroup->getPredictorTangent();
// Compute secant vector
Teuchos::RCP<LOCA::MultiContinuation::ExtendedMultiVector> secant =
Teuchos::rcp_dynamic_cast<LOCA::MultiContinuation::ExtendedMultiVector>(tangent.clone(1));
(*secant)[0].update(1.0, arcLengthGroup->getX(),
-1.0, arcLengthGroup->getPrevX(), 0.0);
// Compute [dx/ds; dp/ds]^T * [x - x_o; p - p_o] - ds
secant->multiply(1.0, scaledTangent, constraints);
for (int i=0; i<arcLengthGroup->getNumParams(); i++)
constraints(i,0) -= arcLengthGroup->getStepSize(i) *
scaledTangent[i].innerProduct(tangent[i]);
isValidConstraints = true;
return finalStatus;
}
NOX::Abstract::Group::ReturnType
NormConstraint::computeDP(const std::vector<int>& paramIDs,
NOX::Abstract::MultiVector::DenseMatrix& dgdp,
bool isValidG)
{
if (!isValidG) {
dgdp(0,0) = constraints(0,0);
}
for (unsigned int i=0; i<paramIDs.size(); i++) {
if (p.getLabel(paramIDs[i]) == "Constraint Param") {
dgdp(0,i+1) = -1.0;
}
else {
dgdp(0,i+1) = 0.0;
}
}
return NOX::Abstract::Group::Ok;
}
void update_constraints (const CosNotifyFilter::Filter_var& filter,
const char* domain1, const char* type1,
const char* domain2, const char* type2)
{
filter->remove_all_constraints ();
CosNotification::EventTypeSeq event_types(2);
event_types.length(2);
event_types[0].domain_name = CORBA::string_dup(domain1);
event_types[0].type_name = CORBA::string_dup(type1);
event_types[1].domain_name = CORBA::string_dup(domain2);
event_types[1].type_name = CORBA::string_dup(type2);
CosNotifyFilter::ConstraintExpSeq constraints(1);
constraints.length(1);
constraints[0].event_types = event_types;
constraints[0].constraint_expr = CORBA::string_dup("");
CosNotifyFilter::ConstraintInfoSeq_var cons_info
= filter->add_constraints(constraints);
std::cout << "Constructing a filter..." << std::endl;
for (CORBA::ULong i = 0; i < event_types.length(); ++i)
{
std::cout << "\tevent_types[" << i << "].domain_name="
<< event_types[i].domain_name
<< std::endl;
std::cout << "\tevent_types[" << i << "].type_name="
<< event_types[i].type_name
<< std::endl;
}
std::cout << "\t**s constraint ="
<< constraints[0].constraint_expr.in ()
<< std::endl;
}
NOX::Abstract::Group::ReturnType
LOCA::MultiContinuation::NaturalConstraint::computeConstraints()
{
if (isValidConstraints)
return NOX::Abstract::Group::Ok;
// Get current, previous solution vectors
const LOCA::MultiContinuation::ExtendedVector& xVec =
dynamic_cast<const LOCA::MultiContinuation::ExtendedVector&>(naturalGroup->
getX());
const LOCA::MultiContinuation::ExtendedVector& prevXVec =
dynamic_cast<const LOCA::MultiContinuation::ExtendedVector&>(naturalGroup->
getPrevX());
for (int i=0; i<naturalGroup->getNumParams(); i++)
constraints(i,0) = xVec.getScalar(i) - prevXVec.getScalar(i) -
naturalGroup->getStepSize(i);
isValidConstraints = true;
return NOX::Abstract::Group::Ok;
}
NOX::Abstract::Group::ReturnType
LOCA::MultiContinuation::NaturalConstraint::computeDP(
const vector<int>& paramIDs,
NOX::Abstract::MultiVector::DenseMatrix& dgdp,
bool isValidG)
{
string callingFunction =
"LOCA::MultiContinuation::NaturalConstraint::computeDP()";
NOX::Abstract::Group::ReturnType status;
NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;
// Compute constraints if necessary
if (!isValidG && !isValidConstraints) {
status = computeConstraints();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
}
if (!isValidG) {
for (int i=0; i<constraints.numRows(); i++)
dgdp(i,0) = constraints(i,0);
}
// If a param ID is equal to a constraint param ID, then that column
// of dgdp is given by that column of the identity matrix, other wise
// that column is zero
vector<int>::const_iterator it;
for (unsigned int i=0; i<paramIDs.size(); i++) {
for (int k=0; k<constraints.numRows(); k++)
dgdp(k,i+1) = 0.0;
it = find(conParamIDs.begin(), conParamIDs.end(), paramIDs[i]);
if (it != conParamIDs.end())
dgdp(it-conParamIDs.begin(),i+1) = 1.0;
}
return finalStatus;
}
void VerletManager::onTick(float seconds){
if(solve){
verlet(seconds);
for(int i=0; i<_numSolves; i++){
_constraintManager->solveConstraints();
constraints();
}
_constraintManager->solveConstraints();
for(Verlet* v:verlets)
v->onTick(seconds);
// foreach(Verlet* v, verlets){
// v->verlet(seconds);
// for(int i=0; i<_numSolves; i++){
// v->linkConstraint();
// v->pinConstraint();
// }
// v->onTick(seconds);
// }
}
}
double symp_euler_step(M& m, double t, double dt, F force, C constraints) {
double mass;
// iterate through each node in the mesh and update position
for(auto it=m.node_begin(); it != m.node_end(); ++ it)
{
(*it).set_position((*it).position() + (*it).value().velocity*dt);
}
// run nodes through constraint functors to correct values for violated
// constraints
constraints(m,t);
// iterate through each node in the mesh and update velocity
for(auto it=m.node_begin(); it != m.node_end(); ++ it)
{
mass = (*it).value().mass;
(*it).value().velocity += force((*it),t)*dt/mass;
}
return t + dt;
}
NOX::Abstract::Group::ReturnType
LOCA::Hopf::MinimallyAugmented::Constraint::
computeConstraints()
{
if (isValidConstraints)
return NOX::Abstract::Group::Ok;
string callingFunction =
"LOCA::Hopf::MinimallyAugmented::Constraint::computeConstraints()";
NOX::Abstract::Group::ReturnType status;
NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;
// Compute C
status = grpPtr->computeComplex(omega);
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
// Compute A and B blocks
Teuchos::RCP<LOCA::Hopf::ComplexMultiVector> A =
Teuchos::rcp(new LOCA::Hopf::ComplexMultiVector(globalData,
(*a_vector)[0], 2));
(*(A->getRealMultiVec()))[0] = (*a_vector)[0];
(*(A->getImagMultiVec()))[0] = (*a_vector)[1];
(*(A->getRealMultiVec()))[1] = (*a_vector)[1];
(*(A->getImagMultiVec()))[1] = (*a_vector)[0];
(*(A->getRealMultiVec()))[1].scale(-1.0);
Teuchos::RCP<LOCA::Hopf::ComplexMultiVector> B =
Teuchos::rcp(new LOCA::Hopf::ComplexMultiVector(globalData,
(*b_vector)[0], 2));
(*(B->getRealMultiVec()))[0] = (*b_vector)[0];
(*(B->getImagMultiVec()))[0] = (*b_vector)[1];
(*(B->getRealMultiVec()))[1] = (*b_vector)[1];
(*(B->getImagMultiVec()))[1] = (*b_vector)[0];
(*(B->getRealMultiVec()))[1].scale(-1.0);
// Set up bordered systems
Teuchos::RCP<const LOCA::BorderedSolver::ComplexOperator> op =
Teuchos::rcp(new LOCA::BorderedSolver::ComplexOperator(grpPtr, omega));
borderedSolver->setMatrixBlocksMultiVecConstraint(op, A, B,
Teuchos::null);
// Create RHS
NOX::Abstract::MultiVector::DenseMatrix one(2,1);
one(0,0) = dn;
one(1,0) = 0.0;
// Get linear solver parameters
Teuchos::RCP<Teuchos::ParameterList> linear_solver_params =
parsedParams->getSublist("Linear Solver");
// Compute sigma_1 and right null vector v
NOX::Abstract::MultiVector::DenseMatrix s1(2,1);
Teuchos::RCP<LOCA::Hopf::ComplexMultiVector> V =
Teuchos::rcp(new LOCA::Hopf::ComplexMultiVector(globalData,
(*v_vector)[0], 1));
(*(V->getRealMultiVec()))[0] = (*v_vector)[0];
(*(V->getImagMultiVec()))[0] = (*v_vector)[1];
status = borderedSolver->initForSolve();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
status = borderedSolver->applyInverse(*linear_solver_params,
NULL,
&one,
*V,
s1);
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
(*v_vector)[0] = (*(V->getRealMultiVec()))[0];
(*v_vector)[1] = (*(V->getImagMultiVec()))[0];
// Teuchos::RCP<NOX::Abstract::MultiVector> rV =
// v_vector->clone(NOX::ShapeCopy);
// NOX::Abstract::MultiVector::DenseMatrix rs1(2,1);
// rV->init(0.0);
// rs1.putScalar(0.0);
// grpPtr->applyComplex((*v_vector)[0], (*v_vector)[1], (*rV)[0], (*rV)[1]);
// (*rV)[0].update(s1(0,0), (*a_vector)[0], -s1(1,0), (*a_vector)[1], 1.0);
// (*rV)[1].update(s1(0,0), (*a_vector)[1], s1(1,0), (*a_vector)[0], 1.0);
// rs1(0,0) = (*b_vector)[0].innerProduct((*v_vector)[0]) +
// (*b_vector)[1].innerProduct((*v_vector)[1]);
// rs1(1,0) = (*b_vector)[0].innerProduct((*v_vector)[1]) -
// (*b_vector)[1].innerProduct((*v_vector)[0]);
// rs1 -= one;
// cout << "rV = " << endl;
// rV->print(cout);
// cout << "rs1 = " << endl;
// rs1.print(cout);
// cout << "checking error..." << endl;
// Teuchos::RCP<NOX::Abstract::MultiVector> rV =
// V->clone(NOX::ShapeCopy);
// NOX::Abstract::MultiVector::DenseMatrix rs1(2,1);
// borderedSolver->apply(*V, s1, *rV, rs1);
// rs1 -= one;
// cout << "rV->norm() = " << (*rV)[0].norm() << endl;
// cout << "rs1.norm() = " << rs1.normInf() << endl;
// Compute sigma_2 and left null vector w
NOX::Abstract::MultiVector::DenseMatrix s2(2,1);
Teuchos::RCP<LOCA::Hopf::ComplexMultiVector> W =
Teuchos::rcp(new LOCA::Hopf::ComplexMultiVector(globalData,
(*w_vector)[0], 1));
(*(W->getRealMultiVec()))[0] = (*w_vector)[0];
(*(W->getImagMultiVec()))[0] = (*w_vector)[1];
if (!isSymmetric) {
status = borderedSolver->initForTransposeSolve();
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
status = borderedSolver->applyInverseTranspose(*linear_solver_params,
NULL,
&one,
*W,
s2);
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
(*w_vector)[0] = (*(W->getRealMultiVec()))[0];
(*w_vector)[1] = (*(W->getImagMultiVec()))[0];
}
else {
*w_vector = *v_vector;
s2.assign(s1);
}
// Teuchos::RCP<NOX::Abstract::MultiVector> rW =
// v_vector->clone(NOX::ShapeCopy);
// NOX::Abstract::MultiVector::DenseMatrix rs2(2,1);
// rW->init(0.0);
// rs2.putScalar(0.0);
// grpPtr->applyComplexTranspose((*w_vector)[0], (*w_vector)[1],
// (*rW)[0], (*rW)[1]);
// (*rW)[0].update(s2(0,0), (*b_vector)[0], -s2(1,0), (*b_vector)[1], 1.0);
// (*rW)[1].update(s2(0,0), (*b_vector)[1], s2(1,0), (*b_vector)[0], 1.0);
// rs2(0,0) = (*a_vector)[0].innerProduct((*w_vector)[0]) +
// (*a_vector)[1].innerProduct((*w_vector)[1]);
// rs2(1,0) = (*a_vector)[0].innerProduct((*w_vector)[1]) -
// (*a_vector)[1].innerProduct((*w_vector)[0]);
// rs2 -= one;
// cout << "rW = " << endl;
// rW->print(cout);
// cout << "rs2 = " << endl;
// rs2.print(cout);
// Teuchos::RCP<NOX::Abstract::MultiVector> rW =
// W->clone(NOX::ShapeCopy);
// NOX::Abstract::MultiVector::DenseMatrix rs2(2,1);
// borderedSolver->applyTranspose(*W, s2, *rW, rs2);
// rs2 -= one;
// cout << "rW->norm() = " << (*rW)[0].norm() << endl;
// cout << "rs2.norm() = " << rs2.normInf() << endl;
// Compute sigma = -w^T*J*v
status = grpPtr->applyComplex((*v_vector)[0], (*v_vector)[1],
(*Cv_vector)[0], (*Cv_vector)[1]);
finalStatus =
globalData->locaErrorCheck->combineAndCheckReturnTypes(status,
finalStatus,
callingFunction);
NOX::Abstract::MultiVector::DenseMatrix tmp(2,2);
Cv_vector->multiply(-1.0, *w_vector, tmp);
constraints(0,0) = tmp(0,0) + tmp(1,1);
constraints(1,0) = tmp(0,1) - tmp(1,0);
// Scale sigma
sigma_scale = dn;
constraints.scale(1.0/sigma_scale);
if (globalData->locaUtils->isPrintType(NOX::Utils::OuterIteration)) {
globalData->locaUtils->out() <<
"\n\tEstimate for singularity of Complex Jacobian (sigma1) =\n\t\t" <<
globalData->locaUtils->sciformat(s1(0,0));
if (s1(1,0) > 0.0)
globalData->locaUtils->out() << " + i ";
else
globalData->locaUtils->out() << " - i ";
globalData->locaUtils->out() <<
globalData->locaUtils->sciformat(std::fabs(s1(1,0)));
globalData->locaUtils->out() <<
"\n\tEstimate for singularity of Complex Jacobian (sigma2) =\n\t\t" <<
globalData->locaUtils->sciformat(s2(0,0));
if (s2(1,0) > 0.0)
globalData->locaUtils->out() << " + i ";
else
globalData->locaUtils->out() << " - i ";
globalData->locaUtils->out() <<
globalData->locaUtils->sciformat(std::fabs(s2(1,0)));
globalData->locaUtils->out() <<
"\n\tEstimate for singularity of Complex Jacobian (sigma ) =\n\t\t" <<
globalData->locaUtils->sciformat(constraints(0,0));
if (constraints(1,0) > 0.0)
globalData->locaUtils->out() << " + i ";
else
globalData->locaUtils->out() << " - i ";
globalData->locaUtils->out() <<
globalData->locaUtils->sciformat(std::fabs(constraints(1,0))) << std::endl;
}
isValidConstraints = true;
// Update a and b if requested
if (updateVectorsEveryIteration) {
if (globalData->locaUtils->isPrintType(NOX::Utils::OuterIteration)) {
globalData->locaUtils->out() <<
"\n\tUpdating null vectors for the next nonlinear iteration" <<
std::endl;
}
*a_vector = *w_vector;
*b_vector = *v_vector;
double a1n = (*a_vector)[0].norm();
double a2n = (*a_vector)[1].norm();
double b1n = (*b_vector)[0].norm();
double b2n = (*b_vector)[1].norm();
a_vector->scale(std::sqrt(dn) / std::sqrt(a1n*a1n + a2n*a2n));
b_vector->scale(std::sqrt(dn) / std::sqrt(b1n*b1n + b2n*b2n));
}
return finalStatus;
}
double
LOCA::Hopf::MinimallyAugmented::Constraint::
getSigmaImag() const
{
return constraints(1,0);
}
int ACE_TMAIN (int argc, ACE_TCHAR *argv[])
{
try
{
PortableServer::POAManager_var poa_manager;
CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
CORBA::Object_var poa_obj = orb->resolve_initial_references("RootPOA");
PortableServer::POA_var root_poa = PortableServer::POA::_narrow(poa_obj.in());
poa_manager = root_poa->the_POAManager();
if (parse_args (argc, argv) != 0)
return 1;
poa_manager->activate();
/*Get event_channel*/
std::cout << "Get event_channel now" << std::endl;
CosNotifyChannelAdmin::EventChannel_var ec = get_event_channel(orb.in());
//Instanciating the Consumer
CosNotifyComm::StructuredPushConsumer_var spc =
CosNotifyComm::StructuredPushConsumer::_nil();
CosNotifyCommImpl::StructuredPushConsumer *pImpl_spc = new CosNotifyCommImpl::StructuredPushConsumer;
spc = pImpl_spc->_this();
//Obtain a Consumer Admin
CosNotifyChannelAdmin::AdminID adminid = 0;
CosNotifyChannelAdmin::ConsumerAdmin_var ca =
ec->new_for_consumers (CosNotifyChannelAdmin::AND_OP, adminid);
if( ca.in() == CosNotifyChannelAdmin::ConsumerAdmin::_nil() ){
std::cerr << "ca is nil!" << std::endl;
return 1;
}
//Obtain a Proxy Consumer
CosNotifyChannelAdmin::ProxyID proxy_id;
CosNotifyChannelAdmin::ClientType ctype = CosNotifyChannelAdmin::STRUCTURED_EVENT;
CosNotifyChannelAdmin::ProxySupplier_var proxySupplier_obj;
try
{
proxySupplier_obj = ca->obtain_notification_push_supplier(ctype, proxy_id);
}
catch(CosNotifyChannelAdmin::AdminLimitExceeded err)
{
std::cerr << "CosNotifyChannelAdmin::AdminLimitExceeded Exception!" << std::endl;
throw;
}
CosNotifyChannelAdmin::StructuredProxyPushSupplier_var pps =
CosNotifyChannelAdmin::StructuredProxyPushSupplier::_narrow(proxySupplier_obj.in());
//Attaching a filter to pps
CosNotifyFilter::FilterFactory_var dff =
ec->default_filter_factory();
ACE_ASSERT(!CORBA::is_nil(dff.in()));
CosNotifyFilter::Filter_var filter = dff->create_filter("EXTENDED_TCL");
CosNotification::EventTypeSeq event_types(1);
event_types.length(2);
event_types[0].domain_name = CORBA::string_dup("DomainA");
event_types[0].type_name = CORBA::string_dup("TypeA");
event_types[1].domain_name = CORBA::string_dup("DomainB");
event_types[1].type_name = CORBA::string_dup("TypeB");
CosNotifyFilter::ConstraintExpSeq constraints(1);
constraints.length(1);
constraints[0].event_types = event_types;
constraints[0].constraint_expr = CORBA::string_dup(
"");
filter->add_constraints(constraints);
pps->add_filter(filter.in());
std::cout << "Attached a filter to ProxyPushSupplier" << std::endl;
std::cout << "The filter's event_types[0].domain_name=" << event_types[0].domain_name << std::endl;
std::cout << "The filter's event_types[0].type_name=" << event_types[0].type_name << std::endl;
std::cout << "The filter's event_types[1].domain_name=" << event_types[1].domain_name << std::endl;
std::cout << "The filter's event_types[1].type_name=" << event_types[1].type_name << std::endl;
//Connecting a Supplier to a Proxy Consumer
try
{
pps->connect_structured_push_consumer(spc.in());
}
catch (CosEventChannelAdmin::AlreadyConnected ac)
{
std::cerr << "CosEventChannelAdmin::AlreadyConnected" << std::endl;
throw;
}
catch (const CORBA::SystemException& se)
{
std::cerr << "System exception occurred during connect: " <<
se << std::endl;
throw;
}
ACE_Time_Value tv (runtime);
orb->run (tv);
ACE_DEBUG ((LM_DEBUG, ACE_TEXT ("Consumer done.\n")));
if (pImpl_spc->received_events ())
{
//Consumer should not receive any events as it's filtered with event type.
std::cerr << "Test failed - received test events." << std::endl;
return 1;
}
else
{
std::cerr << "Test passed - did not receive test events as expected." << std::endl;
}
}
catch(...)
{
std::cerr << "Consumer: Some exceptions was caught!" << std::endl;
return 1;
}
return 0;
}
