frac32buffer centerx
frac32buffer centery
bool32 reset
frac32buffer x1
frac32buffer y1
frac32buffer x2
frac32buffer y2
bool32.mom reset
frac32.s.map x1
frac32.s.map y1
frac32.s.map x1rate
frac32.s.map y1rate
frac32.s.map x2
frac32.s.map y2
frac32.s.map x2rate
frac32.s.map y2rate
frac32.s.map centermw
frac32.s.map.pitch mass1
frac32.s.map.pitch mass2
frac32.s.map.pitch centermass
frac32.u.map damp1
frac32.u.map damp2
int32_t position1x;
int32_t position1y;
int32_t position2x;
int32_t position2y;
float32_t rate1x;
float32_t rate1y;
float32_t rate2x;
float32_t rate2y;
int reset;
int rtrig;
int32_t Damp1x;
int32_t Damp1y;
int32_t Damp2x;
int32_t Damp2y;
reset = 1;
rate1x = 0;
rate1y = 0;
rate2x = 0;
rate2y = 0;
int32_t mass1 = param_mass1;
int32_t mass2 = param_mass2;
int32_t centermass = param_centermass;
MTOFEXTENDED(-mass1, mass1)
MTOFEXTENDED(-mass2, mass2)
MTOFEXTENDED(-centermass, centermass)
int32_t centerx = ___SMMUL(param_centermw << 2, inlet_centerx << 2);
int32_t centery = ___SMMUL(param_centermw << 2, inlet_centery << 2);
float32_t cratio1 =
((float32_t)(centermass + 256)) / ((float32_t)((mass1 >> 8) + 256));
float32_t icratio1 = ((float32_t)(1)) / cratio1;
float32_t cratio2 =
((float32_t)(centermass + 256)) / ((float32_t)((mass2 >> 8) + 256));
float32_t icratio2 = ((float32_t)(1)) / cratio2;
reset = 0;
if ((param_reset || inlet_reset > 0) && (!(rtrig == 1))) {
rtrig = 1;
reset = 1;
} else {
reset = 0;
if (!(param_reset || inlet_reset > 0)) {
rtrig = 0;
}
}
if (reset > 0) {
position1x = param_x1;
position1y = param_y1;
position2x = param_x2;
position2y = param_y2;
rate1x = param_x1rate >> 13;
rate1y = param_y1rate >> 13;
rate2x = param_x2rate >> 13;
rate2y = param_y2rate >> 13;
}
float32_t ratio =
((float32_t)((mass1 >> 8) + 256)) / ((float32_t)((mass2 >> 8) + 256));
float32_t iratio = ((float32_t)(1)) / ratio;
position1x = position1x - ___SMMUL(param_damp1, position1x);
position1y = position1y - ___SMMUL(param_damp1, position1y);
position2x = position2x - ___SMMUL(param_damp2, position2x);
position2y = position2y - ___SMMUL(param_damp2, position2y);
Damp1x = Damp1x + ((position1x - Damp1x) >> 4);
position1x = Damp1x;
Damp1y = Damp1y + ((position1y - Damp1y) >> 4);
position1y = Damp1y;
Damp2x = Damp2x + ((position2x - Damp2x) >> 4);
position2x = Damp2x;
Damp2y = Damp2y + ((position2y - Damp2y) >> 4);
position2y = Damp2y;
int32_t diffx = position1x - position2x;
int32_t diffy = position1y - position2y;
int32_t distance =
___SMMUL(diffx << 3, diffx << 2) + ___SMMUL(diffy << 3, diffy << 2);
float32_t idist = distance;
idist = idist == 0 ? 1 : idist;
idist = _VSQRTF(idist) * (1 << 7);
int32_t xdist1 = position1x - centerx;
int32_t ydist1 = position1y - centery;
xdist1 =
___SMMUL(xdist1 << 3, xdist1 << 2) + ___SMMUL(ydist1 << 3, ydist1 << 2);
float32_t ixdist1 = xdist1;
ixdist1 = ixdist1 == 0 ? 1 : ixdist1;
ixdist1 = _VSQRTF(ixdist1) * (1 << 7);
int32_t xdist2 = position2x - centerx;
int32_t ydist2 = position2y - centery;
xdist2 =
___SMMUL(xdist2 << 3, xdist2 << 2) + ___SMMUL(ydist2 << 3, ydist2 << 2);
float32_t ixdist2 = xdist2;
ixdist2 = ixdist2 == 0 ? 1 : ixdist2;
ixdist2 = _VSQRTF(ixdist2) * (1 << 7);
rate1x -= ((float32_t)(diffx)) / (((float32_t)(idist)) * iratio) +
((float32_t)(+(position1x - centerx))) /
(((float32_t)(ixdist1)) * icratio1);
rate1y -= ((float32_t)(diffy)) / (((float32_t)(idist)) * iratio) +
((float32_t)(+(position1y - centery))) /
(((float32_t)(ixdist1)) * icratio1);
rate2x += ((float32_t)(diffx)) / (((float32_t)(idist)) * ratio) +
((float32_t)(-(position2x - centerx))) /
(((float32_t)(ixdist2)) * icratio2);
rate2y += ((float32_t)(diffy)) / (((float32_t)(idist)) * ratio) +
((float32_t)(-(position2y - centery))) /
(((float32_t)(ixdist2)) * icratio2);
outlet_x1 = position1x += rate1x;
outlet_y1 = position1y += rate1y;
outlet_x2 = position2x += rate2x;
outlet_y2 = position2y += rate2y;
position1x = __SSAT(position1x, 27);
position1y = __SSAT(position1y, 27);
position2x = __SSAT(position2x, 27);
position2y = __SSAT(position2y, 27);