physics-engine/plinks.cpp at staging · ahmadqasembader/physics-engine · GitHub

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
#include "plinks.h"

using Gorgon::Geometry::Point3D;
using namespace Gorgon::Physics;

double ParticleLinks::CurrentLength() const
{
    Point3D relativePos = particle[0]->GetPosition()
                            -  particle[1]->GetPosition();

    return relativePos.Distance();
};

unsigned CableLink::AddContact(ParticleContact *contact, unsigned limit) const
{
    /// Get the current length
    double length = CurrentLength();

    /// Check if the cable is overextended
    if(length < maxLength) return 0;

    contact->particle[0] = particle[0];
    contact->particle[1] = particle[1];

    /// calculate the contact normal
    Point3D normal = particle[1]->GetPosition() - particle[0]->GetPosition();
    normal.Normalize();
    contact->ContactNormal = normal;

    contact->penetration = length - maxLength;
    contact->restitution = restitution;

    return 1;
}

unsigned RodLink::AddContact(ParticleContact *contact, unsigned limit) const
{
    /// Get the current length of the rod
    double currlength = CurrentLength();

    /// Check if the cable is overextended
    if(currlength == length) return 0;

    contact->particle[0] = particle[0];
    contact->particle[1] = particle[1];

    /// Calculate the contact normal
    Point3D normal = particle[1]->GetPosition() - particle[0]->GetPosition();
    normal.Normalize();

    /// The contact normal depends on whether we're extending or compressing
    if (currlength > length) {
        contact->ContactNormal = normal;
        contact->penetration = currlength - length;
    } else {
        contact->ContactNormal = normal * -1;
        contact->penetration = length - currlength;
    }

    /// Always use zero restitution
    contact->restitution = 0;

    return 1;
}

double Constraint::CurrentLength() const
{
    Point3D relativePos = particle->GetPosition() - anchor;
    return relativePos.Distance();
};


unsigned CableConstraints::AddContact(ParticleContact *contact,
                unsigned limit) const
{
    /// Find the current length
    double length = CurrentLength();

    /// Check if we're over-extended or not
    if(length < maxLength) return 0;

    /// Otherwise return the contact
    contact->particle[0] = particle;
    contact->particle[1] = 0;

    /// Calculate the contact normal (collision direction)
    Point3D normal = anchor - particle->GetPosition();
    normal.Normalize();
    contact->ContactNormal = normal;

    contact->restitution = restitution;

    return 1;
}


unsigned RodConstraints::AddContact(ParticleContact *contact,
                                 unsigned limit) const
{
    // Find the length of the rod
    double currentLen = CurrentLength();

    // Check if we're over-extended
    if (currentLen == length) return 0;

    // Otherwise return the contact
    contact->particle[0] = particle;
    contact->particle[1] = 0;

    // Calculate the normal
    Point3D normal = anchor - particle->GetPosition();
    normal.Normalize();

    // The contact normal depends on whether we're extending or compressing
    if (currentLen > length) {
        contact->ContactNormal = normal;
    } else {
        contact->ContactNormal = normal * -1;
    }

    // Always use zero restitution (no bounciness)
    contact->restitution = 0;

    return 1;
}