summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/src/joints/joint.h
blob: b6aa81e15a66586f207e0e441600460da0a0c0df (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

#ifndef _ODE_JOINT_H_
#define _ODE_JOINT_H_


#include <ode/contact.h>
#include "../common.h"
#include "../objects.h"
#include "../obstack.h"


// joint flags
enum
{
    // if this flag is set, the joint was allocated in a joint group
    dJOINT_INGROUP = 1,

    // if this flag is set, the joint was attached with arguments (0,body).
    // our convention is to treat all attaches as (body,0), i.e. so node[0].body
    // is always nonzero, so this flag records the fact that the arguments were
    // swapped.
    dJOINT_REVERSE = 2,

    // if this flag is set, the joint can not have just one body attached to it,
    // it must have either zero or two bodies attached.
    dJOINT_TWOBODIES = 4,

    dJOINT_DISABLED = 8
};


enum dJointConnectedBody
{
    dJCB__MIN,

    dJCB_FIRST_BODY = dJCB__MIN,
    dJCB_SECOND_BODY,

    dJCB__MAX,

};

static inline 
dJointConnectedBody EncodeJointOtherConnectedBody(dJointConnectedBody cbBodyKind)
{
    dIASSERT(dIN_RANGE(cbBodyKind, dJCB__MIN, dJCB__MAX));
    dSASSERT(dJCB__MAX == 2);

    return (dJointConnectedBody)(dJCB_FIRST_BODY + dJCB_SECOND_BODY - cbBodyKind);
}

/* joint body relativity enumeration */
enum dJointBodyRelativity 
{
    dJBR__MIN,

    dJBR_GLOBAL = dJBR__MIN,

    dJBR__BODIES_MIN,

    dJBR_BODY1 = dJBR__BODIES_MIN + dJCB_FIRST_BODY,
    dJBR_BODY2 = dJBR__BODIES_MIN + dJCB_SECOND_BODY,

    dJBR__BODIES_MAX = dJBR__BODIES_MIN + dJCB__MAX,

    dJBR__MAX,

    dJBR__DEFAULT = dJBR_GLOBAL,
    dJBR__BODIES_COUNT = dJBR__BODIES_MAX - dJBR__BODIES_MIN,

};

ODE_PURE_INLINE int dJBREncodeBodyRelativityStatus(int relativity)
{
    return dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX);
}

ODE_PURE_INLINE dJointBodyRelativity dJBRSwapBodyRelativity(int relativity)
{
    dIASSERT(dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX));
    return (dJointBodyRelativity)(dJBR_BODY1 + dJBR_BODY2 - relativity);
}




// there are two of these nodes in the joint, one for each connection to a
// body. these are node of a linked list kept by each body of it's connecting
// joints. but note that the body pointer in each node points to the body that
// makes use of the *other* node, not this node. this trick makes it a bit
// easier to traverse the body/joint graph.

struct dxJointNode
{
    dxJoint *joint;     // pointer to enclosing dxJoint object
    dxBody *body;       // *other* body this joint is connected to
    dxJointNode *next;  // next node in body's list of connected joints
};


struct dxJoint : public dObject
{
    // naming convention: the "first" body this is connected to is node[0].body,
    // and the "second" body is node[1].body. if this joint is only connected
    // to one body then the second body is 0.

    // info returned by getInfo1 function. the constraint dimension is m (<=6).
    // i.e. that is the total number of rows in the jacobian. `nub' is the
    // number of unbounded variables (which have lo,hi = -/+ infinity).

    struct Info1
    {
        // Structure size should not exceed sizeof(pointer) bytes to have 
        // to have good memory pattern in dxQuickStepper()
        uint8 m, nub;
    };

    // info returned by getInfo2 function

    enum
    {
        GI2__J_MIN,
        GI2__JL_MIN = GI2__J_MIN + dDA__L_MIN,

        GI2_JLX = GI2__J_MIN + dDA_LX,
        GI2_JLY = GI2__J_MIN + dDA_LY,
        GI2_JLZ = GI2__J_MIN + dDA_LZ,

        GI2__JL_MAX = GI2__J_MIN + dDA__L_MAX,

        GI2__JA_MIN = GI2__J_MIN + dDA__A_MIN,

        GI2_JAX = GI2__J_MIN + dDA_AX,
        GI2_JAY = GI2__J_MIN + dDA_AY,
        GI2_JAZ = GI2__J_MIN + dDA_AZ,

        GI2__JA_MAX = GI2__J_MIN + dDA__A_MAX,
        GI2__J_MAX = GI2__J_MIN + dDA__MAX,
    };

    enum
    {
        GI2_RHS,
        GI2_CFM,
        GI2__RHS_CFM_MAX,
    };

    enum
    {
        GI2_LO,
        GI2_HI,
        GI2__LO_HI_MAX,
    };

    // info returned by getSureMaxInfo function. 
    // The information is used for memory reservation in calculations.

    struct SureMaxInfo
    {
        // The value of `max_m' must ALWAYS be not less than the value of `m'
        // the getInfo1 call can generate in current joint state. Another 
        // requirement is that the value should be provided very quickly, 
        // without the excessive calculations.
        // If it is hard/impossible to quickly predict the maximal value of `m'
        // (which is the case for most joint types) the maximum for current 
        // joint type in general should be returned. If it can be known the `m'
        // will be smaller, it can save a bit of memory from being reserved 
        // for calculations if that smaller value is returned.

        uint8 max_m; // Estimate of maximal `m' in Info1
    };


    unsigned flags;             // dJOINT_xxx flags
    dxJointNode node[2];        // connections to bodies. node[1].body can be 0
    dJointFeedback *feedback;   // optional feedback structure
    dReal lambda[6];            // lambda generated by last step


    dxJoint( dxWorld *w );
    virtual ~dxJoint();

    bool GetIsJointReverse() const { return (this->flags & dJOINT_REVERSE) != 0; }

    virtual void getInfo1( Info1* info ) = 0;

    // integrator parameters
    virtual void getInfo2( 
        // fps=frames per second (1/stepsize), erp=default error reduction parameter (0..1)
        dReal worldFPS, dReal worldERP, 
        // elements to jump from one row to the next in J's
        int rowskip,
        // for the first and second body, pointers to two (linear and angular)
        // n*3 jacobian sub matrices, stored by rows. these matrices will have
        // been initialized to 0 on entry. if the second body is zero then the
        // J2xx pointers may be 0.
        dReal *J1, dReal *J2,
        // elements to jump from one pair of scalars to the next
        int pairskip,
        // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
        // "constraint force mixing" vector. c is set to zero on entry, cfm is
        // set to a constant value (typically very small or zero) value on entry.
        dReal *pairRhsCfm,
        // lo and hi limits for variables (set to -/+ infinity on entry).
        dReal *pairLoHi,
        // findex vector for variables. see the LCP solver interface for a
        // description of what this does. this is set to -1 on entry.
        // note that the returned indexes are relative to the first index of
        // the constraint.
        int *findex) = 0;
    // This call quickly!!! estimates maximum value of "m" that could be returned by getInfo1()
    // See comments at definition of SureMaxInfo for details.
    virtual void getSureMaxInfo( SureMaxInfo* info ) = 0;
    virtual dJointType type() const = 0;
    virtual sizeint size() const = 0;

    /// Set values which are relative with respect to bodies.
    /// Each dxJoint should redefine it if needed.
    virtual void setRelativeValues();

    // Test if this joint should be used in the simulation step
    // (has the enabled flag set, and is attached to at least one dynamic body)
    bool isEnabled() const;
};


// joint group. NOTE: any joints in the group that have their world destroyed
// will have their world pointer set to 0.

struct dxJointGroup : public dBase
{
    dxJointGroup(): m_num(0), m_stack() {}

    template<class T>
    T *alloc(dWorldID w)
    {
        T *j = (T *)m_stack.alloc(sizeof(T));
        if (j != NULL) {
            ++m_num;
            new(j) T(w);
            j->flags |= dJOINT_INGROUP;
        }
        return j;
    }

    sizeint getJointCount() const { return m_num; }
    sizeint exportJoints(dxJoint **jlist);

    void *beginEnum() { return m_stack.rewind(); }
    void *continueEnum(sizeint num_bytes) { return m_stack.next(num_bytes); }

    void freeAll();

private:
    sizeint m_num;        // number of joints on the stack
    dObStack m_stack; // a stack of (possibly differently sized) dxJoint objects.
};

// common limit and motor information for a single joint axis of movement
struct dxJointLimitMotor
{
    dReal vel, fmax;        // powered joint: velocity, max force
    dReal lostop, histop;   // joint limits, relative to initial position
    dReal fudge_factor;     // when powering away from joint limits
    dReal normal_cfm;       // cfm to use when not at a stop
    dReal stop_erp, stop_cfm; // erp and cfm for when at joint limit
    dReal bounce;           // restitution factor
    // variables used between getInfo1() and getInfo2()
    int limit;          // 0=free, 1=at lo limit, 2=at hi limit
    dReal limit_err;    // if at limit, amount over limit

    void init( dxWorld * );
    void set( int num, dReal value );
    dReal get( int num ) const;
    bool testRotationalLimit( dReal angle );

    enum
    {
        GI2__JL_MIN = dxJoint::GI2__JL_MIN,
        GI2__JA_MIN = dxJoint::GI2__JA_MIN,
        GI2_JAX = dxJoint::GI2_JAX,
        GI2_JAY = dxJoint::GI2_JAY,
        GI2_JAZ = dxJoint::GI2_JAZ,
        GI2_RHS = dxJoint::GI2_RHS,
        GI2_CFM = dxJoint::GI2_CFM,
        GI2_LO = dxJoint::GI2_LO,
        GI2_HI = dxJoint::GI2_HI,
    };

    bool addLimot( dxJoint *joint, dReal fps, 
        dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
        const dVector3 ax1, int rotational );
    bool addTwoPointLimot( dxJoint *joint, dReal fps,
        dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
        const dVector3 ax1, const dVector3 pt1, const dVector3 pt2 );
};


#endif


// Local Variables:
// mode:c++
// c-basic-offset:4
// End: