-
Notifications
You must be signed in to change notification settings - Fork 5
Expand file tree
/
Copy pathJointToPoint.cpp
More file actions
58 lines (48 loc) · 1.68 KB
/
JointToPoint.cpp
File metadata and controls
58 lines (48 loc) · 1.68 KB
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
#include "CPhysics/JointToPoint.h"
#include "CPhysics/Matrix2D.h"
JointToPoint::JointToPoint(Body* b1, const Vectors2D& point, real jointLength, real jointConstant, real dampening,
bool canGoSlack, const Vectors2D& offset1) :
Joint(b1, jointLength, jointConstant, dampening, canGoSlack, offset1)
{
pointAttachedTo = point;
}
JointToPoint::~JointToPoint()
{
object1 = nullptr;
}
const Vectors2D& JointToPoint::getStartPos() const
{
return object1AttachmentPoint;
}
const Vectors2D& JointToPoint::getEndPos() const
{
return pointAttachedTo;
}
void JointToPoint::applyTension()
{
Matrix2D mat1 = Matrix2D(object1->orientation);
this->object1AttachmentPoint = object1->position + (mat1 * offset1);
real tension = calculateTension();
Vectors2D distance = pointAttachedTo - object1AttachmentPoint;
distance.Normalize();
Vectors2D impulse = distance * tension;
object1->applyLinearImpulse(impulse, object1AttachmentPoint - object1->position);
}
real JointToPoint::calculateTension()
{
real distance = (object1AttachmentPoint - pointAttachedTo).len();
if (distance < naturalLength && canGoSlack) {
return 0;
}
real extensionRatio = distance - naturalLength;
real tensionDueToHooksLaw = extensionRatio * springConstant;
real tensionDueToMotionDamping = dampeningConstant * rateOfChangeOfExtension();
return tensionDueToHooksLaw + tensionDueToMotionDamping;
}
real JointToPoint::rateOfChangeOfExtension()
{
Vectors2D distance = pointAttachedTo - object1AttachmentPoint;
distance.Normalize();
Vectors2D relativeVelocity = object1->velocity.negativeVec() - (crossProduct((object1AttachmentPoint - object1->position), object1->angularVelocity));
return dotProduct(relativeVelocity, distance);
}