Exemplo n.º 1
0
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
	MathLib::Point3d const& q, MathLib::Point3d const& r)
{
    MathLib::Vector3 const a(p,r);
    MathLib::Vector3 const b(p,q);

    MathLib::Vector3 const cross_ab(crossProduct(a,b));

    if (cross_ab.getLength() > 0)
    {
        double const denom = 2.0 * scalarProduct(cross_ab,cross_ab);
        MathLib::Vector3 const o = (scalarProduct(b,b) * crossProduct(cross_ab, a)
                                   + scalarProduct(a,a) * crossProduct(b, cross_ab))
                                  * (1.0 / denom);
        _radius = o.getLength() + std::numeric_limits<double>::epsilon();
        _center = MathLib::Vector3(p) + o;
    }
    else
    {
        MinimalBoundingSphere two_pnts_sphere;
        if (a.getLength() > b.getLength())
            two_pnts_sphere = MinimalBoundingSphere(p,r);
        else
            two_pnts_sphere = MinimalBoundingSphere(p,q);
        _radius = two_pnts_sphere.getRadius();
        _center = two_pnts_sphere.getCenter();
    }
}
Exemplo n.º 2
0
MinimalBoundingSphere::MinimalBoundingSphere(
	MathLib::Point3d const& p, MathLib::Point3d const& q)
: _radius(std::numeric_limits<double>::epsilon()), _center(p)
{
    MathLib::Vector3 const a(p, q);

    if (a.getLength() > 0)
    {
        MathLib::Vector3 const o(0.5*a);
        _radius = o.getLength() + std::numeric_limits<double>::epsilon();
        _center = MathLib::Vector3(p) + o;
    }
}
Exemplo n.º 3
0
double sKinematics::CalAngle(MathLib::Vector3 vBase, MathLib::Vector3 vMeasure)
{
	double theta;
	MathLib::Vector3 vOrtho;
	MathLib::Vector3 vDir;
	theta = acos( vBase.Dot(vMeasure) / (vBase.Norm()*vMeasure.Norm()) );

	vOrtho = vBase.Cross(vMeasure);
	vOrtho.Normalize();
	vDir = vOrtho.Cross(vBase);

	if( vDir.Dot(vMeasure) > 0 ) return theta;
	else return -theta;
}
Exemplo n.º 4
0
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
	MathLib::Point3d const& q,
	MathLib::Point3d const& r,
	MathLib::Point3d const& s)
{
    MathLib::Vector3 const a(p, q);
    MathLib::Vector3 const b(p, r);
    MathLib::Vector3 const c(p, s);

    if (!GeoLib::isCoplanar(p, q, r, s))
    {
        double const denom = 2.0 * GeoLib::scalarTriple(a,b,c);
        MathLib::Vector3 const o = (scalarProduct(c,c) * crossProduct(a,b)
                                  + scalarProduct(b,b) * crossProduct(c,a)
                                  + scalarProduct(a,a) * crossProduct(b,c))
                                  * (1.0 / denom);

        _radius = o.getLength() + std::numeric_limits<double>::epsilon();
        _center = MathLib::Vector3(p) + o;
    }
    else
    {
        MinimalBoundingSphere const pqr(p, q , r);
        MinimalBoundingSphere const pqs(p, q , s);
        MinimalBoundingSphere const prs(p, r , s);
        MinimalBoundingSphere const qrs(q, r , s);
        _radius = pqr.getRadius();
        _center = pqr.getCenter();
        if (_radius < pqs.getRadius())
        {
            _radius = pqs.getRadius();
            _center = pqs.getCenter();
        }
        if (_radius < prs.getRadius())
        {
            _radius = prs.getRadius();
            _center = prs.getCenter();
        }
        if (_radius < qrs.getRadius())
        {
            _radius = qrs.getRadius();
            _center = qrs.getCenter();
        }
    }
}
Exemplo n.º 5
0
void MeshSurfaceExtraction::get2DSurfaceElements(const std::vector<MeshLib::Element*> &all_elements, std::vector<MeshLib::Element*> &sfc_elements, const MathLib::Vector3 &dir, double angle, unsigned mesh_dimension)
{
	if (mesh_dimension<2 || mesh_dimension>3)
		ERR("Cannot handle meshes of dimension %i", mesh_dimension);

	bool const complete_surface = (MathLib::scalarProduct(dir, dir) == 0);

	double const pi (boost::math::constants::pi<double>());
	double const cos_theta (std::cos(angle * pi / 180.0));
	MathLib::Vector3 const norm_dir (dir.getNormalizedVector());

	for (auto elem = all_elements.cbegin(); elem != all_elements.cend(); ++elem)
	{
		const unsigned element_dimension ((*elem)->getDimension());
		if (element_dimension < mesh_dimension)
			continue;

		if (element_dimension == 2)
		{
			if (!complete_surface)
			{
				MeshLib::Element* face = *elem;
				if (MathLib::scalarProduct(FaceRule::getSurfaceNormal(face).getNormalizedVector(), norm_dir) > cos_theta)
					continue;
			}
			sfc_elements.push_back(*elem);
		}
		else
		{
			if (!(*elem)->isBoundaryElement())
				continue;
			const unsigned nFaces ((*elem)->getNFaces());
			for (unsigned j=0; j<nFaces; ++j)
			{
				if ((*elem)->getNeighbor(j) != nullptr)
					continue;

				auto const face = std::unique_ptr<MeshLib::Element const>{(*elem)->getFace(j)};
				if (!complete_surface)
				{
					if (MathLib::scalarProduct(FaceRule::getSurfaceNormal(face.get()).getNormalizedVector(), norm_dir) < cos_theta)
					{
						continue;
					}
				}
				if (face->getGeomType() == MeshElemType::TRIANGLE)
					sfc_elements.push_back(new MeshLib::Tri(*static_cast<const MeshLib::Tri*>(face.get())));
				else
					sfc_elements.push_back(new MeshLib::Quad(*static_cast<const MeshLib::Quad*>(face.get())));
			}
		}
	}
}
void    HandSkinControllerThread::PredictPoseFromAngle(){
    // Apply the model and rock the field
    bool bdm = bDebugMode;
    bDebugMode = false;

    if(bDebugMode) cout << "Predicting pose from angle *********"<<endl;


    // Count the number of valid contact

    // Reliability measure for each fingertip
    double  validFTip[FINGERS_COUNT];
    int     validFTipCount = 0;

    if(bNoMissingCheck){
        // If we don't care, let's move on
        for(int i=0;i<FINGERS_COUNT;i++){
            validFTip[i] = 1.0;        
        }
        validFTipCount = FINGERS_COUNT;
    }else{
        for(int i=0;i<FINGERS_COUNT;i++){
            // Higher than min threshold?
            if(mSFingerTip[i][0] >= FTIPTHRESHOLD){
                validFTipCount++;
                // Higher than max threshold?
                if(mSFingerTip[i][0] >= FTIPTHRESHOLDMAX){
                    validFTip[i] = 1.0;
                }else{
                    // Save validity between 0 and 1
                    validFTip[i] = (mSFingerTip[i][0]-FTIPTHRESHOLD)/(FTIPTHRESHOLDMAX-FTIPTHRESHOLD);
                }
            }else{
                validFTip[i] = 0.0;        
            }
        }
    }



    // Additional input weight matrix using the realiability measure
    MathLib::Matrix     inWeights(validFTipCount*3,validFTipCount*3);    
    inWeights.Zero();
    int cnt = 0;
    for(int i=0;i<FINGERS_COUNT;i++){
        if(validFTip[i]>0){
            inWeights(cnt*3+0,i*3+0) = validFTip[i];
            inWeights(cnt*3+1,i*3+1) = validFTip[i];
            inWeights(cnt*3+2,i*3+2) = validFTip[i];
            cnt++;
        }
    }
    
    // Some variables
    MathLib::Vector     inComp(validFTipCount*3);
    MathLib::Vector     outComp(FINGERS_COUNT*2+1+FINGERS_COUNT*1);        
    MathLib::Vector     inV(validFTipCount*3);
    MathLib::Vector     outV(FINGERS_COUNT*2+1+FINGERS_COUNT*1); outV.Zero();
    MathLib::Matrix     sigma(FINGERS_COUNT*2+1+FINGERS_COUNT*1,FINGERS_COUNT*2+1+FINGERS_COUNT*1);

    // Output components indices of the gmm
    for(int i=0;i<outComp.Size();i++)
        outComp(i) = i;
    

    // Input components indices of the gmm
    cnt = 0;
    for(int i=0;i<FINGERS_COUNT;i++){
        if(validFTip[i]>0){
            inComp(cnt++) = outComp.Size()+i*3+0;
            inComp(cnt++) = outComp.Size()+i*3+1;
            inComp(cnt++) = outComp.Size()+i*3+2;
        }
    }

    if(bDebugMode){ cout <<"Vld:  "; for(int i=0;i<FINGERS_COUNT;i++) cout << validFTip[i]<<" "; cout<<endl;}
    if(bDebugMode) cout <<"InC:  " << inComp<<endl;
    
    // Filling the input vector with sensor data
    cnt = 0;
    for(int i=0;i<FINGERS_COUNT;i++){
        if(validFTip[i]>0){
            inV(cnt++) = mSFingerTipDir[i][0];
            inV(cnt++) = mSFingerTipDir[i][1];
            inV(cnt++) = mSFingerTipDir[i][2];
        }
    }

    if(bDebugMode) cout <<"InV:  "<< inV<<endl;


    MathLib::Vector grad,gradNoExp;
    MathLib::Vector inV2,inV0;
    inV2 = inV;
    inV0 = inV;
    

    if(validFTipCount>0){
        // We have at least one valid input
        if(bGradientAscent){
            // We enjoy gradient ascent

            // Get the probability that input query point is in the model            
            double val = mGMModel.GetPx(inV2,inComp,&inWeights);
            
            // Iterate until we're happy
            int iter = 0;
            int iterNoExp = 0;            
            while(1){
                // Get the gradient of current point
                val = mGMModel.GetGradX(inV2,inComp,grad,&inWeights,&gradNoExp);

                // Get the membership of the current input query point, 
                // if higher than threshold we're done 
                if(val >= exp(-0.5*2.5*2.5))
                    break;

                // Norm of the gradient
                double norm = grad.Norm();
                if(val<1e-12){
                    // Too small? use another safer version in gradNoExp
                    grad = gradNoExp;
                    norm = gradNoExp.Norm();
                    norm = 1.0/norm; 

                    iterNoExp++;

                    if(!isnormal(norm)){
                        // Still a not - number... big failure... but we 
                        // exit still...
                        iter = -1;                    
                        break;
                    }
                }else{
                    // Big enough? Normalize
                    norm = 1.0/norm;                
                }
                // Normalize
                grad *= (-norm);
                // Scale the gradient to a little step size and add it 
                // to the query point
                grad.ScaleAddTo(0.01,inV2);

                // Normalization of the query point (i.e. the contact normals
                // should be better unit norm
                int cnt2 = 0;
                for(int i=0;i<FINGERS_COUNT;i++){
                    if(validFTip[i]>0){
                        MathLib::Vector3 vn;
                        vn(0) = inV2(cnt2+0);
                        vn(1) = inV2(cnt2+1);
                        vn(2) = inV2(cnt2+2);
                        vn.Normalize();
                        inV2(cnt2+0) = vn(0);
                        inV2(cnt2+1) = vn(1);
                        inV2(cnt2+2) = vn(2);
                        cnt2 += 3;
                    }
                }

                iter++;
                if(iter>300){
                    // Too much iteration.... again bg failure, exit gracefully
                    break;
                }
            }
            if(bDebugMode) cout << "  Gradient Descent: # iterations : "<<iter<< "("<<iterNoExp<<")"<<endl;


            if((iter<0)||(iter>300)){
                mSFingerDesCoupling.Identity();
                if(bDebugMode){
                    cout << "  Failed: Membership Value (threshold: "<<exp(-0.5*2.5*2.5)<<") -> "<<val <<endl;
                    cout << endl<<endl;
                }
                bDebugMode= bdm;
                return;
            }
        }
    }
    
    
    if(bDebugMode) cout <<"Input: "<< inV<<endl;
    
    // Finally do the regression :)
    inV = inV2;
    double res   = mGMModel.doRegression( inV , inComp, outComp, outV, sigma,&inWeights);

    if(bDebugMode) cout << "Confidence: "<<res<<endl;    
    if(bDebugMode) cout << "Output: "<<outV<<endl;

    // Desired position
    for(int i=0;i<FINGERS_COUNT;i++){
        mSFingerDesPos[i][0] = outV(1+i*2+0);// / mPosFactor;
        mSFingerDesPos[i][1] = outV(1+i*2+1);// / mPosFactor;
    }
    // Desired pressure
    for(int i=0;i<FINGERS_COUNT;i++){
        mSFingerDesTip[i][0] = outV(1+FINGERS_COUNT*2+i)+2.0;
        
        // Tweaking the output sigma matrice (identiting the pressure part,
        // and keeping the position part only (for the force-position control 
        // switch))
        sigma.SetRow(0.0,1+FINGERS_COUNT*2+i);
        sigma.SetColumn(0.0,1+FINGERS_COUNT*2+i);
        sigma(1+FINGERS_COUNT*2+i,1+FINGERS_COUNT*2+i) = 1.0;
    }

    // Inverting the sigma matrix
    MathLib::Matrix nsigma(sigma);
    nsigma.Identity();
    nsigma *= 10.0; // A bit more power in the diagonal
    nsigma += sigma;
        
    // Inverse and store it for later use
    nsigma.InverseSymmetric(mSFingerDesCoupling);    
    

    // More coping fo data here and there
    mJointsTargetPredPos = mJointsRest;
    for(int i=mJointsFingerOffset+1;i<mJointsSize-1;i++){
        mJointsTargetPredPos(i) = outV(i-mJointsFingerOffset-1);
    }

    bDebugMode= bdm;    
}