/usr/include/chipmunk/constraints/util.h is in chipmunk-dev 6.1.5-1.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
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 | /* Copyright (c) 2007 Scott Lembcke
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
// These are utility routines to use when creating custom constraints.
// I'm not sure if this should be part of the private API or not.
// I should probably clean up the naming conventions if it is...
#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(void){return (cpConstraintClass *)&klass;}
void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b);
static inline cpVect
relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
return cpvsub(v2_sum, v1_sum);
}
static inline cpFloat
normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){
return cpvdot(relative_velocity(a, b, r1, r2), n);
}
static inline void
apply_impulse(cpBody *body, cpVect j, cpVect r){
body->v = cpvadd(body->v, cpvmult(j, body->m_inv));
body->w += body->i_inv*cpvcross(r, j);
}
static inline void
apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
{
apply_impulse(a, cpvneg(j), r1);
apply_impulse(b, j, r2);
}
static inline void
apply_bias_impulse(cpBody *body, cpVect j, cpVect r)
{
body->CP_PRIVATE(v_bias) = cpvadd(body->CP_PRIVATE(v_bias), cpvmult(j, body->m_inv));
body->CP_PRIVATE(w_bias) += body->i_inv*cpvcross(r, j);
}
static inline void
apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
{
apply_bias_impulse(a, cpvneg(j), r1);
apply_bias_impulse(b, j, r2);
}
static inline cpFloat
k_scalar_body(cpBody *body, cpVect r, cpVect n)
{
cpFloat rcn = cpvcross(r, n);
return body->m_inv + body->i_inv*rcn*rcn;
}
static inline cpFloat
k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
{
cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n);
cpAssertSoft(value != 0.0, "Unsolvable collision or constraint.");
return value;
}
static inline cpMat2x2
k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2)
{
cpFloat m_sum = a->m_inv + b->m_inv;
// start with Identity*m_sum
cpFloat k11 = m_sum, k12 = 0.0f;
cpFloat k21 = 0.0f, k22 = m_sum;
// add the influence from r1
cpFloat a_i_inv = a->i_inv;
cpFloat r1xsq = r1.x * r1.x * a_i_inv;
cpFloat r1ysq = r1.y * r1.y * a_i_inv;
cpFloat r1nxy = -r1.x * r1.y * a_i_inv;
k11 += r1ysq; k12 += r1nxy;
k21 += r1nxy; k22 += r1xsq;
// add the influnce from r2
cpFloat b_i_inv = b->i_inv;
cpFloat r2xsq = r2.x * r2.x * b_i_inv;
cpFloat r2ysq = r2.y * r2.y * b_i_inv;
cpFloat r2nxy = -r2.x * r2.y * b_i_inv;
k11 += r2ysq; k12 += r2nxy;
k21 += r2nxy; k22 += r2xsq;
// invert
cpFloat det = k11*k22 - k12*k21;
cpAssertSoft(det != 0.0, "Unsolvable constraint.");
cpFloat det_inv = 1.0f/det;
return cpMat2x2New(
k22*det_inv, -k12*det_inv,
-k21*det_inv, k11*det_inv
);
}
static inline cpFloat
bias_coef(cpFloat errorBias, cpFloat dt)
{
return 1.0f - cpfpow(errorBias, dt);
}
|