7 changed files with 119 additions and 202 deletions
@ -0,0 +1,7 @@ |
|||||
|
#ifndef INTERSECTION_H_ |
||||
|
#define INTERSECTION_H_ |
||||
|
|
||||
|
#include "bvh.hpp" |
||||
|
bool CurveCurveBVHIntersect(BVH_AABB_NodePtr &BoxPtr1, BVH_AABB_NodePtr &BoxPtr2, std::vector<std::pair<BVH_AABB_NodePtr, BVH_AABB_NodePtr>> &IstNodePtr); |
||||
|
|
||||
|
#endif |
||||
@ -1,155 +0,0 @@ |
|||||
#include "mathdef.hpp" |
|
||||
/*
|
|
||||
|
|
||||
// 默认id参数不用传，表示四个变量四个方程，id=1 or 2 的时候是2x2的方程组， id=3时是6x6的方程组, id=4 (x0,x1,x2... xn-1) 表示的方程组
|
|
||||
vector<numeric> newton(const MUL_F &F, const vector<numeric> &init_point, const numeric &eps, bool &isConvergence, int id) |
|
||||
{ |
|
||||
vector<symbol> symbols = {get_symbol("u"), get_symbol("v"), get_symbol("s"), get_symbol("t")}; |
|
||||
if (id == 1) |
|
||||
symbols = {get_symbol("u"), get_symbol("v")}; |
|
||||
if (id == 2) |
|
||||
symbols = {get_symbol("s"), get_symbol("t")}; |
|
||||
if (id == 3) |
|
||||
symbols = {get_symbol("u"), get_symbol("v"), get_symbol("s"), get_symbol("t"), get_symbol("p"), get_symbol("q")}; |
|
||||
if (id == 4) |
|
||||
{ |
|
||||
symbols = {}; |
|
||||
for (int i = 0; i < init_point.size(); i++) |
|
||||
{ |
|
||||
symbols.push_back(get_symbol("x" + to_string(i))); |
|
||||
} |
|
||||
} |
|
||||
vector<numeric> x_val = init_point; |
|
||||
vector<numeric> next_x; |
|
||||
|
|
||||
auto checkValue = [&F, &id, &symbols](vector<numeric> &point) |
|
||||
{ |
|
||||
ex res = 0; |
|
||||
lst l = {}; |
|
||||
for (int i = 0; i < F.f.size(); i++) |
|
||||
{ |
|
||||
l.append(symbols[i] == point[i]); |
|
||||
} |
|
||||
for (int i = 0; i < F.f.size(); i++) |
|
||||
{ |
|
||||
ex f = F.f[i].func; |
|
||||
res += abs(evalf(f.subs(l))); |
|
||||
} |
|
||||
return res; |
|
||||
}; |
|
||||
auto derivative = [&symbols](const ex &f, const symbol x, const vector<numeric> &val) |
|
||||
{ |
|
||||
ex diff_f = f.diff(x); |
|
||||
lst L = {}; |
|
||||
for (int i = 0; i < val.size(); i++) |
|
||||
{ |
|
||||
L.append(symbols[i] == val[i]); |
|
||||
} |
|
||||
|
|
||||
ex new_f = evalf(diff_f.subs(L)); |
|
||||
return ex_to<numeric>(new_f); |
|
||||
}; |
|
||||
auto GetJacobinMat = [&derivative, &symbols](const MUL_F &F, const vector<numeric> &xk) |
|
||||
{ |
|
||||
lst l = {}; |
|
||||
int n = xk.size(); |
|
||||
for (int i = 0; i < F.f.size(); i++) |
|
||||
{ |
|
||||
for (int j = 0; j < n; j++) |
|
||||
{ |
|
||||
auto tmp = derivative(F.f[i].func, symbols[j], xk); |
|
||||
l.append(tmp); |
|
||||
} |
|
||||
} |
|
||||
matrix result = matrix(F.f.size(), n, l); |
|
||||
return result; |
|
||||
}; |
|
||||
auto fn = [&symbols](const ex &f, const vector<numeric> &val) |
|
||||
{ |
|
||||
lst L = {}; |
|
||||
for (int i = 0; i < val.size(); i++) |
|
||||
{ |
|
||||
L.append(symbols[i] == val[i]); |
|
||||
} |
|
||||
ex new_f = evalf(f.subs(L)); |
|
||||
return ex_to<numeric>(new_f); |
|
||||
}; |
|
||||
auto GetFk = [&symbols, &fn](const MUL_F &F, const vector<numeric> &xk) |
|
||||
{ |
|
||||
lst l = {}; |
|
||||
int n = F.f.size(); |
|
||||
for (int i = 0; i < n; i++) |
|
||||
{ |
|
||||
auto tmp = fn(F.f[i].func, xk); |
|
||||
l.append(tmp); |
|
||||
} |
|
||||
matrix result = matrix(n, 1, l); |
|
||||
return result; |
|
||||
}; |
|
||||
int n = F.f.size(); |
|
||||
int limit = 300; |
|
||||
isConvergence = true; |
|
||||
numeric totalDelta; |
|
||||
do |
|
||||
{ |
|
||||
next_x.clear(); |
|
||||
matrix grad = GetJacobinMat(F, x_val); |
|
||||
|
|
||||
matrix grad_t = grad.transpose(); |
|
||||
matrix f_xk = GetFk(F, x_val); |
|
||||
|
|
||||
matrix JF_1; |
|
||||
try |
|
||||
{ |
|
||||
JF_1 = grad.inverse(); |
|
||||
} |
|
||||
catch (const std::exception &e) |
|
||||
{ |
|
||||
cout << "grad = " << grad << endl; |
|
||||
JF_1 = grad_t.mul(grad); |
|
||||
isConvergence = false; |
|
||||
for (int i = 0; i < F.f.size(); i++) |
|
||||
{ |
|
||||
cout << "f = " << F.f[i].func << endl; |
|
||||
} |
|
||||
cout << __FILE__ << ":" << __LINE__ << " " << e.what() << endl; |
|
||||
throw runtime_error("need to diff"); |
|
||||
return x_val; |
|
||||
} |
|
||||
|
|
||||
auto result = JF_1.mul(f_xk); |
|
||||
for (int i = 0; i < n; i++) |
|
||||
{ |
|
||||
numeric step = (-ex_to<numeric>(result(i, 0))); |
|
||||
next_x.push_back(x_val[i] + step); |
|
||||
} |
|
||||
totalDelta = 0; |
|
||||
for (int i = 0; i < n; i++) |
|
||||
{ |
|
||||
totalDelta = max(totalDelta, abs(x_val[i] - next_x[i])); |
|
||||
} |
|
||||
|
|
||||
if (totalDelta < eps) |
|
||||
break; |
|
||||
x_val = next_x; |
|
||||
if (checkValue(x_val) < eps) |
|
||||
break; |
|
||||
limit--; |
|
||||
if (!limit) |
|
||||
break; |
|
||||
} while (true); |
|
||||
// cout << "limit = " << 300-limit << endl;
|
|
||||
if (checkValue(x_val) > eps) |
|
||||
{ |
|
||||
isConvergence = false; |
|
||||
// cout << "totaldelta = " << totalDelta << endl;
|
|
||||
// cout << "checkValue = " << checkValue(x_val) << endl;
|
|
||||
} |
|
||||
// if (totalDelta > eps) {
|
|
||||
//
|
|
||||
// isConvergence = false;
|
|
||||
// }
|
|
||||
return x_val; |
|
||||
} |
|
||||
|
|
||||
*/ |
|
||||
Loading…
Reference in new issue