void SceneGraph::link(TransformInstance child, TransformInstance parent)
{
unlink(child);
if (!is_valid(_data.first_child[parent.i]))
{
_data.first_child[parent.i] = child;
_data.parent[child.i] = parent;
}
else
{
TransformInstance prev = { UINT32_MAX };
TransformInstance node = _data.first_child[parent.i];
while (is_valid(node))
{
prev = node;
node = _data.next_sibling[node.i];
}
_data.next_sibling[prev.i] = child;
_data.first_child[child.i].i = UINT32_MAX;
_data.next_sibling[child.i].i = UINT32_MAX;
_data.prev_sibling[child.i] = prev;
}
Matrix4x4 parent_tr = _data.world[parent.i];
Matrix4x4 child_tr = _data.world[child.i];
const Vector3 cs = scale(child_tr);
Vector3 px = x(parent_tr);
Vector3 py = y(parent_tr);
Vector3 pz = z(parent_tr);
Vector3 cx = x(child_tr);
Vector3 cy = y(child_tr);
Vector3 cz = z(child_tr);
set_x(parent_tr, normalize(px));
set_y(parent_tr, normalize(py));
set_z(parent_tr, normalize(pz));
set_x(child_tr, normalize(cx));
set_y(child_tr, normalize(cy));
set_z(child_tr, normalize(cz));
const Matrix4x4 rel_tr = child_tr * get_inverted(parent_tr);
_data.local[child.i].position = translation(rel_tr);
_data.local[child.i].rotation = to_matrix3x3(rel_tr);
_data.local[child.i].scale = cs;
_data.parent[child.i] = parent;
transform(parent_tr, child);
}
Surface& Surface::operator=(const Surface& rSurf)
{
_pMaterialNext=0;
_pMaterialPrev=0;
set_type(rSurf.type());
set_conic(rSurf.conic());
set_radius_curvature(rSurf.radius_curvature());
set_R4(rSurf.R4());
set_R6(rSurf.R6());
set_R8(rSurf.R8());
set_R10(rSurf.R10());
set_comment(rSurf.comment());
set_x(rSurf.x());
set_y(rSurf.y());
set_z(rSurf.z());
set_diameter(rSurf.diameter());
set_auto_diameter(rSurf.get_auto_diameter());
set_inner_diameter(rSurf.inner_diameter());
set_auto_inner_diameter(rSurf.get_auto_inner_diameter());
return *this;
}
void maxCriteria(double x[],double *pfc,double f[],double g[],DOUBLE **dfdx,
int n,int m,int me,double ko[],double komod[],double teta[])
{
double a,*ai;
int i;
double *lb, *rb, Q[_Nq], Ql[_Nq], *dF_dQ, D, Z, **b;
lb = ROPUD_pTK->lbound;
rb = ROPUD_pTK->rbound;
// Приведение к абсолютному значению
// Все поисковые - тетта. Соответственно, x - массив,
// содержащий относительное значение критической точки
for(i=0; i<Nq; i++)
Q[i] = x[i]*(rb[i]-lb[i]) + lb[i];
//f
set_z(ROPUD_pTK, Q);
calcModel(d, ROPUD_pTK->z, Q);
*pfc = calcCriteria();
f[0] = 0;-*pfc;
//f с чертой
for(i=0; i<Nq; i++)
Ql[i] = 0.5*(rb[i]-lb[i]) + lb[i]; // 0.5 - середина
set_z(ROPUD_pTK, Ql);
calcModel(d, ROPUD_pTK->z, Ql);
*pfc -= calcCriteria();
if(is_linearization)
{
dF_dQ = calcDerivative(d, ROPUD_pTK->z, ROPUD_pTK->b, Ql);
for(i=0; i<Nq; i++)
*pfc -= dF_dQ[i]*(Q[i] - Ql[i]);
}
//*pfc=-pow(*pfc, 2);
*pfc=-pow(*pfc, 2);
}
circle::circle(pos p, double radius, double radiussize, gradient grad, int bt)
: shape(bt), gradient_(grad)
{
set_x(p.get_x());
set_y(p.get_y());
set_z(p.get_z());
set_radius(radius);
set_radiussize(radiussize);
}
CMultitaskLogisticRegression::CMultitaskLogisticRegression(
float64_t z, CDotFeatures* train_features,
CBinaryLabels* train_labels, CTaskRelation* task_relation) :
CMultitaskLinearMachine(train_features,(CLabels*)train_labels,task_relation)
{
initialize_parameters();
register_parameters();
set_z(z);
}
void CMultitaskLogisticRegression::initialize_parameters()
{
set_z(0.0);
set_q(2.0);
set_termination(0);
set_regularization(0);
set_tolerance(1e-3);
set_max_iter(1000);
}
//Attach the stickman to the given scene (referenced in gamedialog):
void Stickman::attach_to_scene(QGraphicsScene* scene)
{
scene->addItem(head);
scene->addItem(body);
scene->addItem(left_lower_leg);
scene->addItem(right_lower_leg);
scene->addItem(left_arm);
scene->addItem(right_arm);
set_z(99);
}
void CVector::set_item(int iIndex, float fValue)
{
if (iIndex == 0)
return set_x(fValue);
else if (iIndex == 1)
return set_y(fValue);
else if (iIndex == 2)
return set_z(fValue);
BOOST_RAISE_EXCEPTION(PyExc_IndexError, "Index out of range.")
}
// *****************************************************************************
void stub_PRBF::set(fe_module_t fe_module, concentrator_ID_t concentrator_ID,
fe_chip_ID_t fe_chip_ID, strip_t strip, bend_t bend, z_t z)
{
set_fe_module(fe_module);
set_concentrator_ID(concentrator_ID);
set_fe_chip_ID(fe_chip_ID);
set_strip(strip);
set_bend(bend);
set_z(z);
return;
}
line::line(pos p, pos p2, double size, gradient grad, int blending_type)
: shape(blending_type)
{
set_x(p.get_x());
set_y(p.get_y());
set_z(p.get_z());
set_x2(p2.get_x());
set_y2(p2.get_y());
set_z2(p2.get_z());
set_size(size);
set_gradient(grad);
}
CSLEPMachine::CSLEPMachine(
float64_t z, CDotFeatures* train_features,
CLabels* train_labels) :
CLinearMachine(), m_z(1.0)
{
set_z(z);
set_features(train_features);
set_labels(train_labels);
set_termination(slep_options::get_default_termination());
set_regularization(slep_options::get_default_regularization());
set_tolerance(slep_options::get_default_tolerance());
set_max_iter(slep_options::get_default_max_iter());
}
void maximize(double x[],double *pfc,double f[],double g[],DOUBLE **dfdx,
int n,int m,int me,double ko[],double komod[],double teta[])
{
double *lb, *rb, Q[_Nq];
double D, Z;
int i;
switch(ROPUD_pT->isSoftCons)
{
case 0:
lb = ROPUD_pT->lbound;
rb = ROPUD_pT->rbound;
break;
case 1:
lb = ROPUD_pT->slbound;
rb = ROPUD_pT->srbound;
break;
}
//D = *d;
// Приведение к абсолютному значению
// Все поисковые - тетта. Соответственно, x - массив,
// содержащий относительное значение критической точки
for(i=0; i<Nq; i++)
Q[i] = x[i]*(rb[i]-lb[i]) + lb[i];
set_z(ROPUD_pT->relK, Q);
calcModel(d, ROPUD_pT->relK->z, Q);
//Z = *ROPUD_pT->relK->z;
*pfc = - calcConstraint(ROPUD_pT->NCons);
//switch(ROPUD_pT->NCons)
//{
// // Мягкие
//case 0:
// *pfc = -(-D-Z*Q[0]-2*Q[1]+1); //-D-Z*Q[0]-2*Q[1]+2
// break;
//case 1:
// *pfc = -(-2*D-2*Q[0]-Z*Q[1]+2); //-2*D-2*Q[0]-4*Z*Q[1]+5
// break;
// // Жесткие
//case 2:
// *pfc = -(zmin[0] - Z);
// break;
//case 3:
// *pfc = -(Z - zmax[0]);
// break;
//}
}
void Point_3D::move(Point_3D* origin)
{
//Mise à jour de l'abcisse
set_x(get_x()-origin->get_x());
//Mise à jour de l'ordonnée
set_y(get_y()-origin->get_y());
//Mise à jour de la profondeur
set_z(get_z()-origin->get_z());
//Mise à jour de l'abcisse d'origine
dbl_x_origin-=origin->get_x();
//Mise à jour de l'ordonnée d'origine
dbl_y_origin-=origin->get_y();
//Mise à jour de la profondeur d'origin
dbl_z_origin-=origin->get_z();
}
void Point_3D::scale(int coef)
{
//Mise à jour de l'abcisse
set_x(coef*get_x());
//Mise à jour de l'ordonnée
set_y(coef*get_y());
//Mise à jour de la profondeur
set_z(coef*get_z());
//Mise à jour de l'abcisse d'origine
dbl_x_origin=dbl_x_origin*coef;
//Mise à jour de l'ordonnée d'origine
dbl_y_origin=dbl_y_origin*coef;
//Mise à jour de la profondeur d'origin
dbl_z_origin=dbl_z_origin*coef;
}
Draw_Vector(double const x, double const y, double const z, double const ux, double const uy, double const uz)
{
set_x(x);
set_y(y);
set_z(z);
set_ux(ux);
set_uy(uy);
set_uz(uz);
set_arrow_len(5);
set_arrow_angle(20);
set_arrow_ratio(10);
set_width(1);
set_variable_len(false);
set_color("BLACK");
}
void CollisionPolygon2D::_notification(int p_what) {
switch(p_what) {
case NOTIFICATION_ENTER_TREE: {
unparenting=false;
can_update_body=get_tree()->is_editor_hint();
if (!get_tree()->is_editor_hint()) {
//display above all else
set_z_as_relative(false);
set_z(VS::CANVAS_ITEM_Z_MAX-1);
}
} break;
case NOTIFICATION_EXIT_TREE: {
can_update_body=false;
} break;
case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
if (!is_inside_tree())
break;
if (can_update_body) {
_update_parent();
} else if (shape_from>=0 && shape_to>=0) {
CollisionObject2D *co = get_parent()->cast_to<CollisionObject2D>();
for(int i=shape_from;i<=shape_to;i++) {
co->set_shape_transform(i,get_transform());
}
}
} break;
case NOTIFICATION_DRAW: {
if (!get_tree()->is_editor_hint() && !get_tree()->is_debugging_collisions_hint()) {
break;
}
for(int i=0;i<polygon.size();i++) {
Vector2 p = polygon[i];
Vector2 n = polygon[(i+1)%polygon.size()];
draw_line(p,n,Color(0.9,0.2,0.0,0.8),3);
}
#define DEBUG_DECOMPOSE
#if defined(TOOLS_ENABLED) && defined (DEBUG_DECOMPOSE)
Vector< Vector<Vector2> > decomp = _decompose_in_convex();
Color c(0.4,0.9,0.1);
for(int i=0;i<decomp.size();i++) {
c.set_hsv( Math::fmod(c.get_h() + 0.738,1),c.get_s(),c.get_v(),0.5);
draw_colored_polygon(decomp[i],c);
}
#else
draw_colored_polygon(polygon,get_tree()->get_debug_collisions_color());
#endif
} break;
case NOTIFICATION_UNPARENTED: {
unparenting = true;
_update_parent();
} break;
}
}
/* set f to the rational q */
int
mpfr_set_q (mpfr_ptr f, mpq_srcptr q, mp_rnd_t rnd)
{
mpz_srcptr num, den;
mpfr_t n, d;
int inexact;
int cn, cd;
long shift;
mp_size_t sn, sd;
MPFR_SAVE_EXPO_DECL (expo);
num = mpq_numref (q);
den = mpq_denref (q);
/* NAN and INF for mpq are not really documented, but could be found */
if (MPFR_UNLIKELY (mpz_sgn (num) == 0))
{
if (MPFR_UNLIKELY (mpz_sgn (den) == 0))
{
MPFR_SET_NAN (f);
MPFR_RET_NAN;
}
else
{
MPFR_SET_ZERO (f);
MPFR_SET_POS (f);
MPFR_RET (0);
}
}
if (MPFR_UNLIKELY (mpz_sgn (den) == 0))
{
MPFR_SET_INF (f);
MPFR_SET_SIGN (f, mpz_sgn (num));
MPFR_RET (0);
}
MPFR_SAVE_EXPO_MARK (expo);
cn = set_z (n, num, &sn);
cd = set_z (d, den, &sd);
sn -= sd;
if (MPFR_UNLIKELY (sn > MPFR_EMAX_MAX / BITS_PER_MP_LIMB))
{
inexact = mpfr_overflow (f, rnd, MPFR_SIGN (f));
goto end;
}
if (MPFR_UNLIKELY (sn < MPFR_EMIN_MIN / BITS_PER_MP_LIMB -1))
{
if (rnd == GMP_RNDN)
rnd = GMP_RNDZ;
inexact = mpfr_underflow (f, rnd, MPFR_SIGN (f));
goto end;
}
inexact = mpfr_div (f, n, d, rnd);
shift = BITS_PER_MP_LIMB*sn+cn-cd;
MPFR_ASSERTD (shift == BITS_PER_MP_LIMB*sn+cn-cd);
cd = mpfr_mul_2si (f, f, shift, rnd);
MPFR_SAVE_EXPO_FREE (expo);
if (MPFR_UNLIKELY (cd != 0))
inexact = cd;
else
inexact = mpfr_check_range (f, inexact, rnd);
end:
mpfr_clear (d);
mpfr_clear (n);
return inexact;
}
/* push cells apart or attract them */
void resolve_forces(int iteration)
{
double x1 = get_x();
double y1 = get_y();
double z1 = get_z();
int type1 = get_type();
int num_xy_bonds = 0;
int num_z_bonds = 0;
int num_stem_bonds = 0;
set_force_x(0.0);
set_force_y(0.0);
set_force_z(0.0);
location_message = get_first_location_message();
while (location_message) {
//if (location_message->iteration == iteration){
double x2 = location_message->x;
double y2 = location_message->y;
double z2 = location_message->z;
int type2 = location_message->type;
double distance_check = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + (z1-z2)*(z1-z2));
/*if (distance_check != 0.0)*/
if (distance_check > 0.001 )
{
double force;
double separation_distance = (distance_check - K_WIDTH);
if (separation_distance <= FORCE_IRADIUS) {
if (z2 >= z1) {
if (z2 - z1 > (K_WIDTH/2)) {
num_z_bonds ++;
} else {
num_xy_bonds ++;
}
}
if (location_message->type == K_TYPE_STEM) {
num_stem_bonds ++;
}
if (separation_distance > 0.0) {
force = FORCE_MATRIX[type1][type2];
} else {
force = FORCE_REP;
}
if (on_substrate_surface(z1)) {
force *= DOWNWARD_FORCE[get_type()];
}
force *= FORCE_DAMPENER;
set_force_x(get_force_x() +
force * (separation_distance)*((x2-x1)/distance_check));
set_force_y(get_force_y() +
force * (separation_distance)*((y2-y1)/distance_check));
set_force_z(get_force_z() +
force * (separation_distance)*((z2-z1)/distance_check));
}
}
//}
location_message = get_next_location_message(location_message);
}
/* attraction force to substrate*/
if (z1 <= (K_WIDTH * 1.5)) {
set_force_z(get_force_z() - SUBSTRATE_FORCE[type1]);
}
set_num_xy_bonds(num_xy_bonds);
set_num_z_bonds(num_z_bonds);
set_num_stem_bonds(num_stem_bonds);
x1 += get_force_x();
y1 += get_force_y();
z1 += get_force_z();
if (x1 < 0) {
x1 = 0;
}
if (y1 < 0) {
y1 = 0;
}
if (z1 < 0) {
z1 = 0;
}
if (x1 > substrate_width) {
x1 = substrate_width;
}
if (y1 > substrate_width) {
y1 = substrate_width;
}
set_x(x1);
set_y(y1);
set_z(z1);
}
void IMUReader::run()
{
initSensors();
cs::SensorData sensorMessage;
sensorMessage.set_sensor_id((int) SensorIds::IMU_sensor);
sensorMessage.set_sensor_desc(IMU_SENSOR_NAME);
auto values = sensorMessage.mutable_sensor_values();
auto gyroData = values->Add();
gyroData->set_associated_name("gyro_data");
gyroData->set_data_type(cs::COORD_3D);
auto gyroCoords = gyroData->mutable_coord_value();
int64_t loopCount = 0;
GyroState gyroState = { 0 };
bool calibration = true;
int64_t calibrationDelay = 500;
AHRSProcessor ahrsProcessor(100);
auto instance = ConfigManager::getInstance();
if (!instance)
{
COMM_EXCEPTION(NullPointerException, "Can't get instance of "
"configuration manager");
}
float pitch, roll, yaw;
QUATERNION offsetQ;
showCalibration(true);
auto delayTime = std::chrono::milliseconds(10);
auto t = std::chrono::high_resolution_clock::now();
#if WRITE_SENSORS_DATA
std::ofstream stream("raw_data.txt");
#endif
while (!stopVariable)
{
IMUSensorsData sensorsData = { 0 };
readSensors(sensorsData, gyroState, calibration);
std::this_thread::sleep_until(t + delayTime);
t += delayTime;
#if WRITE_SENSORS_DATA
stream << sensorsData.rawAccelX << " " << sensorsData.rawAccelY <<
" " << sensorsData.rawAccelZ << " " << sensorsData.rawGyroX <<
" " << sensorsData.rawGyroY << " " << sensorsData.rawGyroZ <<
std::endl;
#endif
if (!calibration)
{
ahrsProcessor.updateState(sensorsData, roll, pitch, yaw);
putCurrentAngles(roll, pitch, yaw);
}
else
{
IMUSensorsData tmpData = { 0 };
tmpData.rawAccelX = sensorsData.rawAccelX;
tmpData.rawAccelY = sensorsData.rawAccelY;
tmpData.rawAccelZ = sensorsData.rawAccelZ;
ahrsProcessor.updateState(tmpData, offsetQ);
}
gyroCoords->set_x(roll);
gyroCoords->set_y(pitch);
gyroCoords->set_z(yaw);
int messageSize = sensorMessage.ByteSize();
std::vector<int8_t> outArray(messageSize);
sensorMessage.SerializeToArray(&outArray[0], messageSize);
if (!calibration && loopCount % 10 == 0)
{
//std::cout << sensorsData.rawCompassX << " " << sensorsData.rawCompassY << " " <<
// sensorsData.rawCompassZ << std::endl;
sendData(outArray);
}
loopCount++;
if (loopCount == calibrationDelay)
{
calibration = false;
gyroState.offsetX /= calibrationDelay;
gyroState.offsetY /= calibrationDelay;
gyroState.offsetZ /= calibrationDelay;
ahrsProcessor.setGyroOffsets(gyroState.offsetX, gyroState.offsetY,
gyroState.offsetZ);
ahrsProcessor.setOffsetQuaternion(offsetQ);
showCalibration(false);
}
}
#if WRITE_SENSORS_DATA
stream.close();
#endif
}
void CollisionShape2D::_notification(int p_what) {
switch(p_what) {
case NOTIFICATION_ENTER_TREE: {
unparenting=false;
can_update_body=get_tree()->is_editor_hint();
if (!get_tree()->is_editor_hint()) {
//display above all else
set_z_as_relative(false);
set_z(VS::CANVAS_ITEM_Z_MAX-1);
}
}
break;
case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
if (!is_inside_tree())
break;
if (can_update_body) {
_update_parent();
} else if (update_shape_index>=0) {
CollisionObject2D *co = get_parent()->cast_to<CollisionObject2D>();
if (co) {
co->set_shape_transform(update_shape_index,get_transform());
}
}
}
break;
case NOTIFICATION_EXIT_TREE: {
can_update_body=false;
}
break;
/*
case NOTIFICATION_TRANSFORM_CHANGED: {
if (!is_inside_scene())
break;
_update_parent();
} break;*/
case NOTIFICATION_DRAW: {
if (!get_tree()->is_editor_hint() && !get_tree()->is_debugging_collisions_hint()) {
break;
}
if (!shape.is_valid()) {
break;
}
rect=Rect2();
Color draw_col=get_tree()->get_debug_collisions_color();
shape->draw(get_canvas_item(),draw_col);
rect=shape->get_rect();
rect=rect.grow(3);
}
break;
case NOTIFICATION_UNPARENTED: {
unparenting = true;
_update_parent();
}
break;
}
}
void upperBound(double x[],double *pfc,double f[],double g[],DOUBLE **dfdx,
int n,int m,int me,double ko[],double komod[],double teta[])
{
double D, Z;
double *lb, *rb, *Qcmp, Q[_Nq], **b;
double *pE, ETK[_Nq];
int *piE;
double *dF_dQ, *MQi;
int iz, iq;
struct T *pT;
struct TK *pTK;
int indexq, indext, indexz, indexa,/* indexd,*/ icrit, i_komod, total_krit;
//double EF0, ET0, ETW1, EUU, EKR;
//double VKCE, VKCE0;
//double dconvdF0, dconvdT0, dconvdTW1, dconvdUU, dconvdKR;
//double dT2dF0, dT2dT0, dT2dTW1, dT2dUU, dT2dKR;
//double dQHEdF0, dQHEdT0, dQHEdTW1, dQHEdUU, dQHEdKR;
//double dFwdF0, dFwdT0, dFwdTW1, dFwdUU, dFwdKR;
//double dF1dF0, dF1dT0, dF1dTW1, dF1dUU, dF1dKR;
//double dFW, dF1;
//double *a, *ai;
//struct Q *qi,*qroot=NULL;
//struct Q *qapi,*qaproot=NULL;
double d[_Nd];
int *NQcr;
double U;
int i,j,k,s,ij;
//double *numbers;
double koef;
//double *pQap=NULL;
//double *pZap=NULL;
//double gamma;
double y[_Nq],sig[_Nq],kk;
int nqcr,nqapcr;
int N;
double F;
double *pF=NULL;
*pfc=0;
//Nf = (int)komod[0];
//Nd=(int)komod[2];
//Nt=(int)komod[3];
//Nap=(int)komod[4];
//gamma=komod[5];
//Na=(int)komod[6];
pF=malloc(Nap*sizeof(double));
if(pF==NULL) exit(2);
// читаем D из komod - не как поисковые
//i_komod=7;
// заполняем массив числа крит точек по областям
//NQcr=malloc(Nt*sizeof(int));
//if(NQcr==NULL) exit(2);
////for(indext=0;indext<Nt;indext++)
//// NQcr[indext]=0;
//total_krit=0;
//for(indext=0;indext<Nt;indext++)
//{
// NQcr[indext]=(int)komod[i_komod];
// total_krit+=NQcr[indext];
// i_komod++;
//}
//qroot=malloc(sizeof(struct Q));
//if(qroot==NULL) exit(2);
//qroot->next=NULL;
//qi=qroot;
//i=0;
//nqcr=0;
//for(indext=0; indext<Nt; indext++)
//{
// icrit = NQcr[indext];
// for(indexq=0; indexq<icrit; indexq++)
// {
// qi->next = malloc(sizeof(struct Q));
// if(!qi->next) exit(2);
// qi = qi->next;
// qi->next = NULL;
// qi->F0 = komod[i_komod++];
// qi->T0 = komod[i_komod++];
// //qi->TW1 = komod[i_komod++];
// //qi->UU = komod[i_komod++];
// //qi->KR = komod[i_komod++];
// qi->T=indext;
// nqcr++;
// }
//}
//qaproot=malloc(sizeof(struct Q));
//if(qaproot==NULL) exit(2);
//qaproot->next=NULL;
//qapi=qaproot;
//i=0;
//nqapcr=0;
//for(indexq=0;indexq<Nap;indexq++)
//{
// qapi->next=malloc(sizeof(struct Q));
// if(qapi->next==NULL) exit(2);
// qapi=qapi->next;
// qapi->next=NULL;
// qapi->F0 = komod[i_komod++];
// qapi->T0 = komod[i_komod++];
// //qapi->TW1 = komod[i_komod++];
// //qapi->UU = komod[i_komod++];
// //qapi->KR = komod[i_komod++];
// qapi->T = indexq;
// nqapcr++;
//}
kk = 3.9; // Правило трех сигм
for(i=0; i<Nq; i++)
sig[i] = (start_q[i]-start_minq[i])/kk;
ij = Nd;
pT = ROPUD_pT;
while(pT=pT->pnext)
if(pT->isSoftCons)
{
for(iq=0; iq<Nq; iq++,ij++)
{
pT->slbound[iq] = x[ij];
pT->srbound[iq] = x[ij+Nq];
}
ij+=Nq;
};
// Копирование значений b
pTK = ROPUD_pTK;
while(pTK=pTK->pnext)
for(iz=0; iz<Nz; iz++)
for(iq=0; iq<Nq+1; iq++, ij++)
pTK->b[iz][iq] = x[ij];
//ограничения
//ограничения Эф Круглое
for(i=0; i<Na; i++)
f[i] = alpha[i];
pT = ROPUD_pT;
while(pT=pT->pnext)
{
if(pT->isSoftCons)
{
for (j=0, F=1; j<Nq; j++)
F *= (Fx((pT->srbound[j]-start_q[j])/sig[j]) - Fx((pT->slbound[j]-start_q[j])/sig[j]));
f[pT->NCons] -= F;
}
}
//f[0]*=20;
//f[1]*=200;
//f[3]*=200;
//f[4]*=200;
//вероятностные
i = Na;
//qi = qroot;
pT = ROPUD_pT;
for(indext=0; indext<Nt; indext++)
{
pT = pT->pnext;
// Ограничения на области по каждому параметру (правая граница больше левой)
if(pT->isSoftCons)
{
for(j=0; j<Nq; j++,i++)
f[i] = pT->slbound[j] - pT->srbound[j];
//f[i-4] /= 10.0; f[i-3] /= 10.0;
//f[i-2] *= 100.0; f[i-1] *= 1000.0;
}
//while((qi->next)&&(qi->next->T==indext))
for(icrit=0; icrit<pT->NQcr; icrit++)
{
//qi=qi->next;
//Q[2] = qi->TW1;
//Q[3] = qi->UU;
//Q[4] = qi->KR;
//Q = pT->Qcr + icrit*Nq;
Qcmp = pT->Qcr + icrit*Nq;
lb = pT->slbound;
rb = pT->srbound;
//if(pT->isSoftCons)
for(iq=0; iq<Nq; iq++)
Q[iq] = Qcmp[iq]*(rb[iq]-lb[iq]) + lb[iq];
//else
//for(iq=0; iq<Nq; iq++)
// Q[iq] = Q[iq]*(pT->rbound[iq] - pT->lbound[iq]) + pT->lbound[iq];
set_z(pT->relK, Q);
calcModel(x, pT->relK->z, Q);
//D = x[0];
//Z = pT->relK->z[0];
//switch(pT->NCons)
//{
// // Мягкие
//case 0:
// f[i] = -D-Z*Q[0]-2*Q[1]+1; //-D-Z*Q[0]-2*Q[1]+2
// break;
//case 1:
// f[i] = -2*D-2*Q[0]-Z*Q[1]+2; //-2*D-2*Q[0]-4*Z*Q[1]+5
// break;
// // Жесткие
//case 2:
// f[i] = zmin[0] - Z;
// break;
//case 3:
// f[i] = Z - zmax[0];
// break;
//}
////if(pT->isLocked) f[i] = 0;
//i++;
f[i++] = calcConstraint(pT->NCons);
//f[i-1] *= 50;
}
}
j = 0;
pTK = ROPUD_pTK;
while(pTK = pTK->pnext)
{
pE = pTK->E;
piE = pTK->iE;
for(iq=0; iq<Nq; iq++)
Q[iq] = (pTK->lbound[iq] + pTK->rbound[iq]) /2;
set_z(pTK, Q);
calcModel(x, pTK->z, Q);
//D = x[0];
//Z = pTK->z[0];
b = pTK->b;
//dF_dQ = malloc(2*sizeof(double));
//dF_dQ[0] = D + 0.5*Q[1]*b[0][0];
//dF_dQ[1] = 0.5*(Z + b[0][1]*Q[1]);
//**************************************************************
pF[j] = calcCriteria() * pTK->a;
if(is_linearization)
{
dF_dQ = calcDerivative(x, pTK->z, pTK->b, Q);
for(iq=0; iq<Nq; iq++)
pF[j] += dF_dQ[iq]*(*pE++-Q[iq]*pTK->ai[iq]);
}
//pF[j] += dF_dQ[0]*(*pE++-Q[0]*pTK->ai[0]);
//pF[j] += dF_dQ[1]*(*pE++-Q[1]*pTK->ai[1]);
//f[i++] = zmin[0] - *pTK->z;
//f[i++] = *pTK->z - zmax[0];
for(iz=0; iz<Nz*2; iz++)
f[i++] = calcConstraintOnZ(iz);
//f[i-1] *= 100;
//f[i-2] /= 100.0;
//f[i-3] *= 100;
//f[i-4] /= 100.0;
//if(D<0.6)
// D=D;
j++;
}
//if(Nap==1) *pfc = *pF;
//else
{
for(j=0;j<Nap;j++)
*pfc+=pF[j];
}
f[i++] = -*pfc;
//for(i=0; i<Nf; i++)
// //if(f[i]>0) f[i]+=1000;
// while(f[i]<0.1 && f[i]>0.00001) f[i]*=10;
//if(*pfc<0)
// printf("%f\t%f",T1,T2);
//qi=qroot;
//while(qi)
//{
// qi=qi->next;
// free(qroot);
// qroot=NULL;
// qroot=qi;
//}
//qapi=qaproot;
//while(qapi)
//{
// qapi=qapi->next;
// free(qaproot);
// qaproot=NULL;
// qaproot=qapi;
//}
//free(NQcr);
free(pF);
}
