← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/Body/PenetrationBlocker.cpp

  • Committer: Thomas Moulard
  • Date: 2012-10-23 12:43:24 UTC
  • Revision ID: git-v1:351cf736ad49bc7a9a7b9767dee760a013517a5d
Tags: upstream/1.1.0
Imported Upstream version 1.1.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Body/PenetrationBlocker.cpp
 
1
/**
 
2
   @author Shin'ichiro Nakaoka
 
3
*/
 
4
 
 
5
#include "PenetrationBlocker.h"
 
6
#include <cnoid/ColdetModelPair>
 
7
 
 
8
using namespace std;
 
9
using namespace boost;
 
10
using namespace cnoid;
 
11
 
 
12
namespace {
 
13
    const bool TRACE_FUNCTIONS = false;
 
14
}
 
15
 
 
16
namespace cnoid {
 
17
 
 
18
    class PenetrationBlockerImpl
 
19
    {
 
20
    public:
 
21
        PenetrationBlockerImpl(Link* targetLink);
 
22
 
 
23
        Link* targetLink;
 
24
        ColdetModelPtr targetModel;
 
25
        std::vector<ColdetModelPairPtr> coldetModelPairs;
 
26
        double targetDepth;
 
27
        Vector3 pPrevGiven;
 
28
        bool isPrevBlocked;
 
29
        Vector3 prevBlockedPosition;
 
30
        Vector3 prevBlockedNormal;
 
31
 
 
32
        struct LinkInfo {
 
33
            Link* link;
 
34
            // A coldet model is copied from the original one
 
35
            ColdetModelPtr coldetModel;
 
36
        };
 
37
        std::vector<LinkInfo> opponentLinkInfos;
 
38
 
 
39
        void addOpponentLink(Link* link);
 
40
 
 
41
        void start();
 
42
        bool adjust(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection);
 
43
        bool adjustSub(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection);
 
44
    };
 
45
}
 
46
 
 
47
 
 
48
PenetrationBlocker::PenetrationBlocker(Link* targetLink)
 
49
{
 
50
    impl = new PenetrationBlockerImpl(targetLink);
 
51
}
 
52
 
 
53
 
 
54
PenetrationBlockerImpl::PenetrationBlockerImpl(Link* targetLink)
 
55
    : targetLink(targetLink)
 
56
{
 
57
    if(targetLink->coldetModel){
 
58
        targetModel = new ColdetModel(*targetLink->coldetModel);
 
59
    }
 
60
    pPrevGiven = targetLink->p;
 
61
 
 
62
    targetDepth = 0.001;
 
63
    isPrevBlocked = false;
 
64
}
 
65
 
 
66
 
 
67
void PenetrationBlocker::addOpponentLink(Link* link)
 
68
{
 
69
    impl->addOpponentLink(link);
 
70
}
 
71
 
 
72
 
 
73
void PenetrationBlockerImpl::addOpponentLink(Link* link)
 
74
{
 
75
    if(link->coldetModel){
 
76
        ColdetModelPtr opponendModel = new ColdetModel(*link->coldetModel);
 
77
        coldetModelPairs.push_back(new ColdetModelPair(opponendModel, targetModel));
 
78
        LinkInfo info;
 
79
        info.link = link;
 
80
        info.coldetModel = opponendModel;
 
81
        opponentLinkInfos.push_back(info);
 
82
    }
 
83
}
 
84
 
 
85
 
 
86
void PenetrationBlocker::setDepth(double depth)
 
87
{
 
88
    impl->targetDepth = depth;
 
89
}
 
90
 
 
91
 
 
92
void PenetrationBlocker::start()
 
93
{
 
94
    impl->isPrevBlocked = false;
 
95
}
 
96
 
 
97
 
 
98
bool PenetrationBlocker::adjust(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection)
 
99
{
 
100
    return impl->adjust(io_p, R, pushDirection);
 
101
}
 
102
 
 
103
 
 
104
bool PenetrationBlockerImpl::adjust(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection)
 
105
{
 
106
    if(isPrevBlocked){
 
107
        if((io_p - prevBlockedPosition).dot(prevBlockedNormal) < 0.0){
 
108
            io_p = prevBlockedPosition;
 
109
            return true;
 
110
        }
 
111
    }
 
112
    return adjustSub(io_p, R, pushDirection);
 
113
}
 
114
 
 
115
 
 
116
bool PenetrationBlockerImpl::adjustSub(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection)
 
117
{
 
118
    bool blocked = false;
 
119
    const Vector3 s = pushDirection.normalized();
 
120
    
 
121
    for(size_t i=0; i < opponentLinkInfos.size(); ++i){
 
122
        LinkInfo& info = opponentLinkInfos[i];
 
123
        info.coldetModel->setPosition(info.link->R, info.link->p);
 
124
    }
 
125
 
 
126
    int loop;
 
127
    Vector3 maxnormal(Vector3::Zero());
 
128
    
 
129
    for(loop = 0; loop < 100; ++loop){
 
130
 
 
131
        targetModel->setPosition(R, io_p);
 
132
 
 
133
        double maxsdepth = 0.0;
 
134
        double maxdepth = 0.0;
 
135
        
 
136
        for(size_t i=0; i < coldetModelPairs.size(); ++i){
 
137
            ColdetModelPair& modelPair = *coldetModelPairs[i];
 
138
            const std::vector<collision_data>& cols = modelPair.detectCollisions();
 
139
            for(size_t j=0; j < cols.size(); ++j){
 
140
                const collision_data& col = cols[j];
 
141
                if(col.depth > targetDepth){
 
142
                    double d = -col.n_vector.dot(s);
 
143
                    if(d > 0.0){
 
144
                        double sdepth = col.depth * d;
 
145
                        if(sdepth > maxsdepth){
 
146
                            maxsdepth = sdepth;
 
147
                            maxdepth = col.depth;
 
148
                            maxnormal = col.n_vector;
 
149
                        }
 
150
                    }
 
151
                }
 
152
            }
 
153
        }
 
154
        
 
155
        if(maxsdepth > 0.0){
 
156
            io_p += (maxdepth - targetDepth) * maxnormal;
 
157
            blocked = true;
 
158
        } else {
 
159
            break;
 
160
        }
 
161
    }
 
162
 
 
163
    isPrevBlocked = blocked;
 
164
    prevBlockedPosition = io_p;
 
165
    prevBlockedNormal = maxnormal;
 
166
 
 
167
    return blocked;
 
168
}