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;
}
Beispiel #3
0
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);
    }
}
Beispiel #4
0
/** @brief Returns the norm of the orbital */
double QMFunction::norm() const {
    double norm = squaredNorm();
    if (norm > 0.0) norm = std::sqrt(norm);
    return norm;
}
Beispiel #5
0
double complex::norm() const
{
    return sqrt(squaredNorm());
}
Beispiel #6
0
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;
 }