bool SegmentSegmentCcdStaticContact::collideStaticPointSegment(
const Math::Vector3d& point,
const std::array<SurgSim::Math::Vector3d, 2>& p,
double thicknessPoint, double thicknessSegment,
double* r)
{
Math::Vector3d b = p[0];
Math::Vector3d c = p[1];
auto ba = point - b;
auto ca = point - c;
auto bc = c - b;
double baNormSQ = ba.squaredNorm();
double caNormSQ = ca.squaredNorm();
double bcNormSQ = bc.squaredNorm();
double totalThicknessSQ = (thicknessPoint + thicknessSegment) * (thicknessPoint + thicknessSegment);
// p is essentially a point
if (bcNormSQ < m_degenerateEpsilon)
{
if (baNormSQ <= totalThicknessSQ)
{
*r = 0.0;
return true;
}
if (caNormSQ <= totalThicknessSQ)
{
*r = 1.0;
return true;
}
else
{
return false;
}
}
// b!=c => compute the projection abscissa
*r = bc.dot(ba) / bcNormSQ;
// Clamp abscissa
*r = Math::clamp(*r, 0.0, 1.0, 0.0);
// Compute the closest point of a on [bc]
Math::Vector3d closestPtOnBC = Math::interpolate(b, c, *r);
return (closestPtOnBC - point).squaredNorm() <= totalThicknessSQ;
}
jfloatArray Java_fr_bowserf_nbodyproblem_CalculationNDK_computeNewPosition(JNIEnv *env, jobject obj){
int numberCoordinates = N * 3;
float *newPositions = (float*)malloc((numberCoordinates) * sizeof(float));
for(int i = 0 ; i < N ; i++){
float *acc = (float*)calloc(3, sizeof(float));
for(int j = 0 ; j < N ; j++){
float square = squaredNorm(p + i * 3, p + j * 3);
float denominateur = (float) pow(square + squaredEpsilon, THREE_AND_HALF);
float* resultSub = subtraction(p + i * 3, p + j * 3);
float* numerateur = mult(m[j], resultSub);
float* resultMult = mult(1 / denominateur, numerateur);
add(acc, resultMult);
free(resultSub);
free(numerateur);
free(resultMult);
}
float *tmp_v = addition(v + i * 3, mult(G, acc));
v[i * 3] = tmp_v[0];
v[i * 3 + 1] = tmp_v[1];
v[i * 3 + 2] = tmp_v[2];
float *tmp_rep = addition(v + i * 3, p + i * 3);
newPositions[i * 3] = tmp_rep[0];
newPositions[i * 3 + 1] = tmp_rep[1];
newPositions[i * 3 + 2] = tmp_rep[2];
free(tmp_rep);
free(acc);
free(tmp_v);
}
for(int i = 0 ; i < numberCoordinates ; i++){
p[i] = newPositions[i];
}
free(newPositions);
(*env)->SetFloatArrayRegion(env, result, 0, numberCoordinates, p);
return result;
}
void abcd::aijAugmentMatrix(std::vector<CompCol_Mat_double> &M)
{
int nbcols = A.dim(1);
std::map<int,std::vector<CompCol_Mat_double> > C;
std::map<int,std::vector<int> > stCols;
#ifdef WIP
double filter_c = dcntl[Controls::aug_filter];
#endif //WIP
stC.assign(M.size(), -1);
for( size_t i = 0; i < M.size() - 1; i++ ){
for ( size_t j = i+1; j < M.size(); j++ ) {
std::vector<int> intersect;
std::set_intersection(column_index[i].begin(),
column_index[i].end(),
column_index[j].begin(),
column_index[j].end(),
std::back_inserter(intersect));
if (intersect.empty()) continue;
CompCol_Mat_double A_ij(sub_matrix(M[i], intersect));
CompCol_Mat_double A_ji(sub_matrix(M[j], intersect));
double *jv = A_ji.val_ptr();
for (int k = 0; k < A_ji.NumNonzeros(); k++) {
jv[k] *= -1.0;
}
#ifdef WIP
if(filter_c != 0 || icntl[Controls::aug_iterative] != 0) {
std::vector<int> selected_cols;
std::vector<double> frob_ij, mu;
frob_ij.reserve(A_ij.dim(1));
mu.reserve(A_ij.dim(1));
double card_max = 0;
double frob_sum = 0;
double nu;
for (int k = 0; k < A_ij.dim(1); ++k){
VECTOR_int A_ij_k_ind, A_ji_k_ind;
VECTOR_double A_ij_k = middleCol(A_ij, k, A_ij_k_ind);
VECTOR_double A_ji_k = middleCol(A_ji, k, A_ji_k_ind);
double card_current = A_ij_k_ind.size() * A_ji_k_ind.size();
// exploit the sparcity of the vectors!
frob_ij.push_back(sqrt( squaredNorm(A_ij_k, A_ij_k_ind) * squaredNorm(A_ji_k, A_ji_k_ind)));
frob_sum += frob_ij[k];
card_max = card_max > card_current ? card_max : card_current;
}
nu = (frob_sum / frob_ij.size()) / sqrt(card_max);
for (int k = 0; k < A_ij.dim(1); ++k){
VECTOR_int A_ij_k_ind, A_ji_k_ind;
VECTOR_double A_ij_k = middleCol(A_ij, k, A_ij_k_ind);
VECTOR_double A_ji_k = middleCol(A_ji, k, A_ji_k_ind);
double inf_ij = infNorm(A_ij_k);
double inf_ji = infNorm(A_ji_k);
double p = 0, q = 0;
for (int l = 0; l < A_ij_k_ind.size(); ++l){
if (abs(A_ij_k(A_ij_k_ind(l))) >= nu/inf_ji) p++;
}
for (int l = 0; l < A_ji_k_ind.size(); ++l){
if (abs(A_ji_k(A_ji_k_ind(l))) >= nu/inf_ij) q++;
}
p = ( p==0 ? A_ij_k_ind.size() : p );
q = ( q==0 ? A_ji_k_ind.size() : q );
double mu_ij_k = frob_ij[k] / sqrt(p*q);
mu.push_back(mu_ij_k);
if(mu_ij_k >= filter_c){
selected_cols.push_back(k);
}
if(icntl[Controls::aug_iterative] != 0){
if (dcntl[Controls::aug_precond] < 0) {
if ((nbcols + k - n_o) % abs((int)dcntl[Controls::aug_precond]) == 0)
selected_S_columns.push_back( nbcols + k - n_o);
else
skipped_S_columns.push_back( nbcols + k - n_o);
} else {
if (mu_ij_k >= dcntl[Controls::aug_precond])
selected_S_columns.push_back( nbcols + k - n_o);
else
skipped_S_columns.push_back( nbcols + k - n_o);
}
}
}
if (selected_cols.empty()) continue;
if( icntl[Controls::aug_iterative] != 2 ) { // don't reduce the A_ij/A_ji, we just need the selected columns!
A_ij = sub_matrix(A_ij, selected_cols);
A_ji = sub_matrix(A_ji, selected_cols);
}
}
#endif //WIP
stCols[i].push_back(nbcols);
stCols[j].push_back(nbcols);
C[i].push_back(A_ij);
C[j].push_back(A_ji);
nbcols += A_ij.dim(1);
}
}
size_c = nbcols - A.dim(1);
n = nbcols;
LINFO << "Size of C : " << size_c;
#ifdef WIP
if(icntl[Controls::aug_analysis] != 0) return;
#endif // WIP
// Augment the matrices
for(size_t k = 0; k < M.size(); k++){
if(stCols[k].size() == 0) continue;
// now augment each partition!
stC[k] = stCols[k][0];
M[k] = concat_columns(M[k], C[k], stCols[k]);
M[k] = resize_columns(M[k], nbcols);
}
}
/** @brief Returns the norm of the orbital */
double QMFunction::norm() const {
double norm = squaredNorm();
if (norm > 0.0) norm = std::sqrt(norm);
return norm;
}
double complex::norm() const
{
return sqrt(squaredNorm());
}
typename VectorBase<scalar,index,SizeAtCompileTime>::podscalar VectorBase<scalar,index,SizeAtCompileTime>::normalize()
{
podscalar norm = std::sqrt(squaredNorm());
scale(1.0/norm);
return norm;
}
Image<T, D> squaredNorm(const Image<Matrix<T,M,N>, D>& src)
{
Image<T, D> sqNorm;
squaredNorm(sqNorm, src);
return sqNorm;
}