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

use na::{RealField, Unit};
use ncollide::query::ContactKinematic;

use crate::math::Vector;
use crate::object::{BodyHandle, BodyPartHandle, BodySet, ColliderHandle};
use crate::solver::IntegrationParameters;

/// A generic non-linear position constraint.
pub struct GenericNonlinearConstraint<N: RealField, Handle: BodyHandle> {
    /// The first body affected by the constraint.
    pub body1: BodyPartHandle<Handle>,
    /// The second body affected by the constraint.
    pub body2: Option<BodyPartHandle<Handle>>,
    /// Whether this constraint affects the bodies translation or orientation.
    pub is_angular: bool,
    //  FIXME: rename ndofs1?
    /// Number of degree of freedom of the first body.
    pub dim1: usize,
    /// Number of degree of freedom of the second body.
    pub dim2: usize,
    /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the first body.
    pub wj_id1: usize,
    /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the second body.
    pub wj_id2: usize,
    /// The target position change this constraint must apply.
    pub rhs: N,
    /// The scaling parameter of the SOR-prox method.
    pub r: N,
}

impl<N: RealField, Handle: BodyHandle> GenericNonlinearConstraint<N, Handle> {
    /// Initialize a new nonlinear constraint.
    pub fn new(
        body1: BodyPartHandle<Handle>,
        body2: Option<BodyPartHandle<Handle>>,
        is_angular: bool,
        dim1: usize,
        dim2: usize,
        wj_id1: usize,
        wj_id2: usize,
        rhs: N,
        r: N,
    ) -> Self {
        GenericNonlinearConstraint {
            body1,
            body2,
            is_angular,
            dim1,
            dim2,
            wj_id1,
            wj_id2,
            rhs,
            r,
        }
    }
}

/// Implemented by structures that generate non-linear constraints.
pub trait NonlinearConstraintGenerator<N: RealField, Handle: BodyHandle> {
    /// Maximum of non-linear position constraint this generator needs to output.
    fn num_position_constraints(&self, bodies: &dyn BodySet<N, Handle = Handle>) -> usize;
    /// Generate the `i`-th position constraint of this generator.
    fn position_constraint(
        &self,
        parameters: &IntegrationParameters<N>,
        i: usize,
        bodies: &mut dyn BodySet<N, Handle = Handle>,
        jacobians: &mut [N],
    ) -> Option<GenericNonlinearConstraint<N, Handle>>;
}

/// A non-linear position-based non-penetration constraint.
#[derive(Debug)]
pub struct NonlinearUnilateralConstraint<
    N: RealField,
    Handle: BodyHandle,
    CollHandle: ColliderHandle,
> {
    /// The scaling parameter of the SOR-prox method.
    pub r: N,
    /// The target position change this constraint must apply.
    pub rhs: N,

    /// Number of degree of freedom of the first body.
    pub ndofs1: usize,
    /// The first body affected by the constraint.
    pub body1: BodyPartHandle<Handle>,
    /// The first collider affected by the constraint.
    pub collider1: CollHandle,

    /// Number of degree of freedom of the second body.
    pub ndofs2: usize,
    /// The second body affected by the constraint.
    pub body2: BodyPartHandle<Handle>,
    /// The second collider affected by the constraint.
    pub collider2: CollHandle,

    /// The kinematic information used to update the contact location.
    pub kinematic: ContactKinematic<N>,

    /// The contact normal on the local space of `self.body1`.
    pub normal1: Unit<Vector<N>>,
    /// The contact normal on the local space of `self.body2`.
    pub normal2: Unit<Vector<N>>,
}

impl<N: RealField, Handle: BodyHandle, CollHandle: ColliderHandle>
    NonlinearUnilateralConstraint<N, Handle, CollHandle>
{
    /// Create a new nonlinear position-based non-penetration constraint.
    pub fn new(
        body1: BodyPartHandle<Handle>,
        collider1: CollHandle,
        ndofs1: usize,
        body2: BodyPartHandle<Handle>,
        collider2: CollHandle,
        ndofs2: usize,
        normal1: Unit<Vector<N>>,
        normal2: Unit<Vector<N>>,
        kinematic: ContactKinematic<N>,
    ) -> Self {
        let r = N::zero();
        let rhs = N::zero();

        NonlinearUnilateralConstraint {
            r,
            rhs,
            ndofs1,
            body1,
            collider1,
            ndofs2,
            body2,
            collider2,
            kinematic,
            normal1,
            normal2,
        }
    }
}

/// A non-linear position constraint generator to enforce multibody joint limits.
pub struct MultibodyJointLimitsNonlinearConstraintGenerator;

impl MultibodyJointLimitsNonlinearConstraintGenerator {
    /// Creates the constraint generator from the given multibody link.
    pub fn new() -> Self {
        MultibodyJointLimitsNonlinearConstraintGenerator
    }
}

impl<N: RealField, Handle: BodyHandle> NonlinearConstraintGenerator<N, Handle>
    for MultibodyJointLimitsNonlinearConstraintGenerator
{
    fn num_position_constraints(&self, _: &dyn BodySet<N, Handle = Handle>) -> usize {
        0
    }

    fn position_constraint(
        &self,
        _: &IntegrationParameters<N>,
        _: usize,
        _: &mut dyn BodySet<N, Handle = Handle>,
        _: &mut [N],
    ) -> Option<GenericNonlinearConstraint<N, Handle>> {
        None
    }
}