|
@ -20,20 +20,27 @@ public: |
|
|
vectorNew[1] = parameters[2]*(vectorOld[0] + parameters[3]); |
|
|
vectorNew[1] = parameters[2]*(vectorOld[0] + parameters[3]); |
|
|
vectorNew[2] = parameters[4]*(vectorOld[1] + parameters[5]); |
|
|
vectorNew[2] = parameters[4]*(vectorOld[1] + parameters[5]); |
|
|
|
|
|
|
|
|
const double x = vectorNew[0] / parameters[6]; |
|
|
const double x = vectorNew[0]; |
|
|
const double y = vectorNew[1] / parameters[6]; |
|
|
const double y = vectorNew[1]; |
|
|
const double z = vectorNew[2] / parameters[6]; |
|
|
const double z = vectorNew[2]; |
|
|
const double dist = x*x + y*y + 2.0*z*z; |
|
|
const double dist = x*x + y*y + 2.0*z*z - 1.0;//parameters[7];
|
|
|
|
|
|
const double lh = dist*dist*dist; |
|
|
if(parameters[7] * dist - parameters[7] > x*x*y*y*y) { |
|
|
const double rh = parameters[6]*x*x*y*y*y; |
|
|
const double sqrtDist = std::sqrt(dist); |
|
|
|
|
|
const double p = 1.0 - parameters[6] * (static_cast<int>(sqrtDist / parameters[6]) + 1.0) / sqrtDist; |
|
|
if(lh > rh) { |
|
|
|
|
|
const double sqrtDist = std::pow(std::abs(lh), 1.0/6.0); |
|
|
|
|
|
const double sqrtDist2 = std::pow(std::abs(rh), 1.0/6.0) * sign(parameters[6]); |
|
|
|
|
|
const double p = 1.0 - sqrtDist2 * (static_cast<int>(sqrtDist / sqrtDist2) + 1.0) / sqrtDist; |
|
|
vectorNew[0] *= p; |
|
|
vectorNew[0] *= p; |
|
|
vectorNew[1] *= p; |
|
|
vectorNew[1] *= -p; |
|
|
vectorNew[2] *= p; |
|
|
vectorNew[2] *= p; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
double sign(double x){ |
|
|
|
|
|
return (x > 0) - (x < 0); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
}; |
|
|
}; |
|
|
|
|
|
|
|
|
#endif // UNRAVEL_HPP
|
|
|
#endif // UNRAVEL_HPP
|
|
|