blob: f8be49aa126258025f4d77d3beeffef69e4e9d24 (
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
|
#include "SM_FhObject.h"
#include "MT_MinMax.h"
void SM_FhObject::ray_hit(void *client_data,
void *client_object1,
void *client_object2,
const DT_CollData *coll_data) {
SM_Object *hit_object = (SM_Object *)client_object1;
const SM_MaterialProps *matProps = hit_object->getMaterialProps();
if ((matProps == 0) || (matProps->m_fh_distance < MT_EPSILON)) {
return;
}
SM_FhObject *fh_object = (SM_FhObject *)client_object2;
SM_Object *cl_object = fh_object->getClientObject();
if (hit_object == cl_object) {
// Shot myself in the foot...
return;
}
const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
// Exit if the client object is not dynamic.
if (shapeProps == 0) {
return;
}
MT_Point3 lspot;
MT_Vector3 normal;
if (DT_ObjectRayTest(hit_object->getObjectHandle(),
fh_object->getPosition().getValue(),
fh_object->getSpot().getValue(),
lspot.getValue(), normal.getValue())) {
const MT_Vector3& ray_dir = fh_object->getRayDirection();
MT_Scalar dist = MT_distance(fh_object->getPosition(),
hit_object->getWorldCoord(lspot)) -
cl_object->getMargin();
normal.normalize();
if (dist < matProps->m_fh_distance) {
if (shapeProps->m_do_fh) {
MT_Vector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocity(lspot);
MT_Scalar rel_vel_ray = ray_dir.dot(rel_vel);
MT_Scalar spring_extent = 1.0 - dist / matProps->m_fh_distance;
MT_Scalar i_spring = spring_extent * matProps->m_fh_spring;
MT_Scalar i_damp = rel_vel_ray * matProps->m_fh_damping;
cl_object->addLinearVelocity(-(i_spring + i_damp) * ray_dir);
if (matProps->m_fh_normal) {
cl_object->addLinearVelocity(
(i_spring + i_damp) *
(normal - normal.dot(ray_dir) * ray_dir));
}
MT_Vector3 lateral = rel_vel - rel_vel_ray * ray_dir;
const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
if (shapeProps->m_do_anisotropic) {
MT_Matrix3x3 lcs(cl_object->getOrientation());
MT_Vector3 loc_lateral = lateral * lcs;
const MT_Vector3& friction_scaling =
shapeProps->m_friction_scaling;
loc_lateral.scale(friction_scaling[0],
friction_scaling[1],
friction_scaling[2]);
lateral = lcs * loc_lateral;
}
MT_Scalar rel_vel_lateral = lateral.length();
if (rel_vel_lateral > MT_EPSILON) {
MT_Scalar friction_factor = matProps->m_friction;
MT_Scalar max_friction = friction_factor * MT_max(0.0, i_spring);
MT_Scalar rel_mom_lateral = rel_vel_lateral /
cl_object->getInvMass();
MT_Vector3 friction =
(rel_mom_lateral > max_friction) ?
-lateral * (max_friction / rel_vel_lateral) :
-lateral;
cl_object->applyCenterImpulse(friction);
}
}
if (shapeProps->m_do_rot_fh) {
const double *ogl_mat = cl_object->getMatrix();
MT_Vector3 up(&ogl_mat[8]);
MT_Vector3 t_spring = up.cross(normal) * matProps->m_fh_spring;
MT_Vector3 ang_vel = cl_object->getAngularVelocity();
// only rotations that tilt relative to the normal are damped
ang_vel -= ang_vel.dot(normal) * normal;
MT_Vector3 t_damp = ang_vel * matProps->m_fh_damping;
cl_object->addAngularVelocity(t_spring - t_damp);
}
}
}
}
|
