aboutsummaryrefslogtreecommitdiff
path: root/servers
diff options
context:
space:
mode:
Diffstat (limited to 'servers')
-rw-r--r--servers/physics/body_pair_sw.cpp1
-rw-r--r--servers/physics/body_sw.h1
-rw-r--r--servers/physics/shape_sw.cpp3272
-rw-r--r--servers/physics_server.cpp1
-rw-r--r--servers/physics_server.h1
-rw-r--r--servers/visual/rasterizer.h2
-rw-r--r--servers/visual/shader_language.cpp6
-rw-r--r--servers/visual/visual_server_raster.cpp27
-rw-r--r--servers/visual/visual_server_raster.h8
-rw-r--r--servers/visual/visual_server_wrap_mt.h2
-rw-r--r--servers/visual_server.h3
11 files changed, 1683 insertions, 1641 deletions
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index 094f56a42..c50bfac47 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -227,6 +227,7 @@ bool BodyPairSW::setup(float p_step) {
227 Vector3 global_A = xform_Au.xform(c.local_A); 227 Vector3 global_A = xform_Au.xform(c.local_A);
228 Vector3 global_B = xform_Bu.xform(c.local_B); 228 Vector3 global_B = xform_Bu.xform(c.local_B);
229 229
230
230 real_t depth = c.normal.dot(global_A - global_B); 231 real_t depth = c.normal.dot(global_A - global_B);
231 232
232 if (depth<=0) { 233 if (depth<=0) {
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 892389927..8d3006977 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -323,6 +323,7 @@ public:
323 virtual Transform get_transform() const { return body->get_transform(); } 323 virtual Transform get_transform() const { return body->get_transform(); }
324 324
325 virtual void add_force(const Vector3& p_force, const Vector3& p_pos) { body->add_force(p_force,p_pos); } 325 virtual void add_force(const Vector3& p_force, const Vector3& p_pos) { body->add_force(p_force,p_pos); }
326 virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j) { body->apply_impulse(p_pos,p_j); }
326 327
327 virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } 328 virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
328 virtual bool is_sleeping() const { return !body->is_active(); } 329 virtual bool is_sleeping() const { return !body->is_active(); }
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
index 70e5c00b9..1312b7da9 100644
--- a/servers/physics/shape_sw.cpp
+++ b/servers/physics/shape_sw.cpp
@@ -26,1639 +26,1639 @@
26/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ 26/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
27/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ 27/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
28/*************************************************************************/ 28/*************************************************************************/
29#include "shape_sw.h" 29#include "shape_sw.h"
30#include "geometry.h" 30#include "geometry.h"
31#include "sort.h" 31#include "sort.h"
32#include "quick_hull.h" 32#include "quick_hull.h"
33#define _POINT_SNAP 0.001953125 33#define _POINT_SNAP 0.001953125
34#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002 34#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002
35#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998 35#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998
36 36
37 37
38void ShapeSW::configure(const AABB& p_aabb) { 38void ShapeSW::configure(const AABB& p_aabb) {
39 aabb=p_aabb; 39 aabb=p_aabb;
40 configured=true; 40 configured=true;
41 for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) { 41 for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) {
42 ShapeOwnerSW* co=(ShapeOwnerSW*)E->key(); 42 ShapeOwnerSW* co=(ShapeOwnerSW*)E->key();
43 co->_shape_changed(); 43 co->_shape_changed();
44 } 44 }
45} 45}
46 46
47 47
48Vector3 ShapeSW::get_support(const Vector3& p_normal) const { 48Vector3 ShapeSW::get_support(const Vector3& p_normal) const {
49 49
50 Vector3 res; 50 Vector3 res;
51 int amnt; 51 int amnt;
52 get_supports(p_normal,1,&res,amnt); 52 get_supports(p_normal,1,&res,amnt);
53 return res; 53 return res;
54} 54}
55 55
56void ShapeSW::add_owner(ShapeOwnerSW *p_owner) { 56void ShapeSW::add_owner(ShapeOwnerSW *p_owner) {
57 57
58 Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner); 58 Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
59 if (E) { 59 if (E) {
60 E->get()++; 60 E->get()++;
61 } else { 61 } else {
62 owners[p_owner]=1; 62 owners[p_owner]=1;
63 } 63 }
64} 64}
65 65
66void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){ 66void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){
67 67
68 Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner); 68 Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
69 ERR_FAIL_COND(!E); 69 ERR_FAIL_COND(!E);
70 E->get()--; 70 E->get()--;
71 if (E->get()==0) { 71 if (E->get()==0) {
72 owners.erase(E); 72 owners.erase(E);
73 } 73 }
74 74
75} 75}
76 76
77bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{ 77bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{
78 78
79 return owners.has(p_owner); 79 return owners.has(p_owner);
80 80
81} 81}
82 82
83const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{ 83const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{
84 return owners; 84 return owners;
85} 85}
86 86
87 87
88ShapeSW::ShapeSW() { 88ShapeSW::ShapeSW() {
89 89
90 custom_bias=0; 90 custom_bias=0;
91 configured=false; 91 configured=false;
92} 92}
93 93
94 94
95ShapeSW::~ShapeSW() { 95ShapeSW::~ShapeSW() {
96 96
97 ERR_FAIL_COND(owners.size()); 97 ERR_FAIL_COND(owners.size());
98} 98}
99 99
100 100
101 101
102Plane PlaneShapeSW::get_plane() const { 102Plane PlaneShapeSW::get_plane() const {
103 103
104 return plane; 104 return plane;
105} 105}
106 106
107void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 107void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
108 108
109 // gibberish, a plane is infinity 109 // gibberish, a plane is infinity
110 r_min=-1e7; 110 r_min=-1e7;
111 r_max=1e7; 111 r_max=1e7;
112} 112}
113 113
114Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const { 114Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const {
115 115
116 return p_normal*1e15; 116 return p_normal*1e15;
117} 117}
118 118
119 119
120bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 120bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
121 121
122 bool inters=plane.intersects_segment(p_begin,p_end,&r_result); 122 bool inters=plane.intersects_segment(p_begin,p_end,&r_result);
123 if(inters) 123 if(inters)
124 r_normal=plane.normal; 124 r_normal=plane.normal;
125 return inters; 125 return inters;
126} 126}
127 127
128Vector3 PlaneShapeSW::get_moment_of_inertia(float p_mass) const { 128Vector3 PlaneShapeSW::get_moment_of_inertia(float p_mass) const {
129 129
130 return Vector3(); //wtf 130 return Vector3(); //wtf
131} 131}
132 132
133void PlaneShapeSW::_setup(const Plane& p_plane) { 133void PlaneShapeSW::_setup(const Plane& p_plane) {
134 134
135 plane=p_plane; 135 plane=p_plane;
136 configure(AABB(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2))); 136 configure(AABB(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2)));
137} 137}
138 138
139void PlaneShapeSW::set_data(const Variant& p_data) { 139void PlaneShapeSW::set_data(const Variant& p_data) {
140 140
141 _setup(p_data); 141 _setup(p_data);
142 142
143} 143}
144 144
145Variant PlaneShapeSW::get_data() const { 145Variant PlaneShapeSW::get_data() const {
146 146
147 return plane; 147 return plane;
148} 148}
149 149
150PlaneShapeSW::PlaneShapeSW() { 150PlaneShapeSW::PlaneShapeSW() {
151 151
152 152
153} 153}
154 154
155// 155//
156 156
157float RayShapeSW::get_length() const { 157float RayShapeSW::get_length() const {
158 158
159 return length; 159 return length;
160} 160}
161 161
162void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 162void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
163 163
164 // don't think this will be even used 164 // don't think this will be even used
165 r_min=0; 165 r_min=0;
166 r_max=1; 166 r_max=1;
167} 167}
168 168
169Vector3 RayShapeSW::get_support(const Vector3& p_normal) const { 169Vector3 RayShapeSW::get_support(const Vector3& p_normal) const {
170 170
171 if (p_normal.z>0) 171 if (p_normal.z>0)
172 return Vector3(0,0,length); 172 return Vector3(0,0,length);
173 else 173 else
174 return Vector3(0,0,0); 174 return Vector3(0,0,0);
175} 175}
176 176
177void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { 177void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
178 178
179 if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { 179 if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
180 180
181 r_amount=2; 181 r_amount=2;
182 r_supports[0]=Vector3(0,0,0); 182 r_supports[0]=Vector3(0,0,0);
183 r_supports[1]=Vector3(0,0,length); 183 r_supports[1]=Vector3(0,0,length);
184 } if (p_normal.z>0) { 184 } if (p_normal.z>0) {
185 r_amount=1; 185 r_amount=1;
186 *r_supports=Vector3(0,0,length); 186 *r_supports=Vector3(0,0,length);
187 } else { 187 } else {
188 r_amount=1; 188 r_amount=1;
189 *r_supports=Vector3(0,0,0); 189 *r_supports=Vector3(0,0,0);
190 } 190 }
191} 191}
192 192
193 193
194bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 194bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
195 195
196 return false; //simply not possible 196 return false; //simply not possible
197} 197}
198 198
199Vector3 RayShapeSW::get_moment_of_inertia(float p_mass) const { 199Vector3 RayShapeSW::get_moment_of_inertia(float p_mass) const {
200 200
201 return Vector3(); 201 return Vector3();
202} 202}
203 203
204void RayShapeSW::_setup(float p_length) { 204void RayShapeSW::_setup(float p_length) {
205 205
206 length=p_length; 206 length=p_length;
207 configure(AABB(Vector3(0,0,0),Vector3(0.1,0.1,length))); 207 configure(AABB(Vector3(0,0,0),Vector3(0.1,0.1,length)));
208} 208}
209 209
210void RayShapeSW::set_data(const Variant& p_data) { 210void RayShapeSW::set_data(const Variant& p_data) {
211 211
212 _setup(p_data); 212 _setup(p_data);
213 213
214} 214}
215 215
216Variant RayShapeSW::get_data() const { 216Variant RayShapeSW::get_data() const {
217 217
218 return length; 218 return length;
219} 219}
220 220
221RayShapeSW::RayShapeSW() { 221RayShapeSW::RayShapeSW() {
222 222
223 length=1; 223 length=1;
224} 224}
225 225
226 226
227 227
228/********** SPHERE *************/ 228/********** SPHERE *************/
229 229
230real_t SphereShapeSW::get_radius() const { 230real_t SphereShapeSW::get_radius() const {
231 231
232 return radius; 232 return radius;
233} 233}
234 234
235void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 235void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
236 236
237 float d = p_normal.dot( p_transform.origin ); 237 float d = p_normal.dot( p_transform.origin );
238 238
239 // figure out scale at point 239 // figure out scale at point
240 Vector3 local_normal = p_transform.basis.xform_inv(p_normal); 240 Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
241 float scale = local_normal.length(); 241 float scale = local_normal.length();
242 242
243 r_min = d - (radius) * scale; 243 r_min = d - (radius) * scale;
244 r_max = d + (radius) * scale; 244 r_max = d + (radius) * scale;
245 245
246} 246}
247 247
248Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const { 248Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const {
249 249
250 return p_normal*radius; 250 return p_normal*radius;
251} 251}
252 252
253void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { 253void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
254 254
255 *r_supports=p_normal*radius; 255 *r_supports=p_normal*radius;
256 r_amount=1; 256 r_amount=1;
257} 257}
258 258
259bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 259bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
260 260
261 return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal); 261 return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal);
262} 262}
263 263
264Vector3 SphereShapeSW::get_moment_of_inertia(float p_mass) const { 264Vector3 SphereShapeSW::get_moment_of_inertia(float p_mass) const {
265 265
266 float s = 0.4 * p_mass * radius * radius; 266 float s = 0.4 * p_mass * radius * radius;
267 return Vector3(s,s,s); 267 return Vector3(s,s,s);
268} 268}
269 269
270void SphereShapeSW::_setup(real_t p_radius) { 270void SphereShapeSW::_setup(real_t p_radius) {
271 271
272 272
273 radius=p_radius; 273 radius=p_radius;
274 configure(AABB( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0))); 274 configure(AABB( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0)));
275 275
276} 276}
277 277
278void SphereShapeSW::set_data(const Variant& p_data) { 278void SphereShapeSW::set_data(const Variant& p_data) {
279 279
280 _setup(p_data); 280 _setup(p_data);
281} 281}
282 282
283Variant SphereShapeSW::get_data() const { 283Variant SphereShapeSW::get_data() const {
284 284
285 return radius; 285 return radius;
286} 286}
287 287
288SphereShapeSW::SphereShapeSW() { 288SphereShapeSW::SphereShapeSW() {
289 289
290 radius=0; 290 radius=0;
291} 291}
292 292
293 293
294/********** BOX *************/ 294/********** BOX *************/
295 295
296 296
297void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 297void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
298 298
299 // no matter the angle, the box is mirrored anyway 299 // no matter the angle, the box is mirrored anyway
300 Vector3 local_normal=p_transform.basis.xform_inv(p_normal); 300 Vector3 local_normal=p_transform.basis.xform_inv(p_normal);
301 301
302 float length = local_normal.abs().dot(half_extents); 302 float length = local_normal.abs().dot(half_extents);
303 float distance = p_normal.dot( p_transform.origin ); 303 float distance = p_normal.dot( p_transform.origin );
304 304
305 r_min = distance - length; 305 r_min = distance - length;
306 r_max = distance + length; 306 r_max = distance + length;
307 307
308 308
309} 309}
310 310
311Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const { 311Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const {
312 312
313 313
314 Vector3 point( 314 Vector3 point(
315 (p_normal.x<0) ? -half_extents.x : half_extents.x, 315 (p_normal.x<0) ? -half_extents.x : half_extents.x,
316 (p_normal.y<0) ? -half_extents.y : half_extents.y, 316 (p_normal.y<0) ? -half_extents.y : half_extents.y,
317 (p_normal.z<0) ? -half_extents.z : half_extents.z 317 (p_normal.z<0) ? -half_extents.z : half_extents.z
318 ); 318 );
319 319
320 return point; 320 return point;
321} 321}
322 322
323void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { 323void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
324 324
325 static const int next[3]={1,2,0}; 325 static const int next[3]={1,2,0};
326 static const int next2[3]={2,0,1}; 326 static const int next2[3]={2,0,1};
327 327
328 for (int i=0;i<3;i++) { 328 for (int i=0;i<3;i++) {
329 329
330 Vector3 axis; 330 Vector3 axis;
331 axis[i]=1.0; 331 axis[i]=1.0;
332 float dot = p_normal.dot( axis ); 332 float dot = p_normal.dot( axis );
333 if ( Math::abs( dot ) > _FACE_IS_VALID_SUPPORT_TRESHOLD ) { 333 if ( Math::abs( dot ) > _FACE_IS_VALID_SUPPORT_TRESHOLD ) {
334 334
335 //Vector3 axis_b; 335 //Vector3 axis_b;
336 336
337 bool neg = dot<0; 337 bool neg = dot<0;
338 r_amount = 4; 338 r_amount = 4;
339 339
340 Vector3 point; 340 Vector3 point;
341 point[i]=half_extents[i]; 341 point[i]=half_extents[i];
342 342
343 int i_n=next[i]; 343 int i_n=next[i];
344 int i_n2=next2[i]; 344 int i_n2=next2[i];
345 345
346 static const float sign[4][2]={ 346 static const float sign[4][2]={
347 347
348 {-1.0, 1.0}, 348 {-1.0, 1.0},
349 { 1.0, 1.0}, 349 { 1.0, 1.0},
350 { 1.0,-1.0}, 350 { 1.0,-1.0},
351 {-1.0,-1.0}, 351 {-1.0,-1.0},
352 }; 352 };
353 353
354 for (int j=0;j<4;j++) { 354 for (int j=0;j<4;j++) {
355 355
356 point[i_n]=sign[j][0]*half_extents[i_n]; 356 point[i_n]=sign[j][0]*half_extents[i_n];
357 point[i_n2]=sign[j][1]*half_extents[i_n2]; 357 point[i_n2]=sign[j][1]*half_extents[i_n2];
358 r_supports[j]=neg?-point:point; 358 r_supports[j]=neg?-point:point;
359 359
360 } 360 }
361 361
362 if (neg) { 362 if (neg) {
363 SWAP( r_supports[1], r_supports[2] ); 363 SWAP( r_supports[1], r_supports[2] );
364 SWAP( r_supports[0], r_supports[3] ); 364 SWAP( r_supports[0], r_supports[3] );
365 } 365 }
366 366
367 return; 367 return;
368 } 368 }
369 369
370 r_amount=0; 370 r_amount=0;
371 371
372 } 372 }
373 373
374 for (int i=0;i<3;i++) { 374 for (int i=0;i<3;i++) {
375 375
376 Vector3 axis; 376 Vector3 axis;
377 axis[i]=1.0; 377 axis[i]=1.0;
378 378
379 if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) { 379 if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) {
380 380
381 r_amount= 2; 381 r_amount= 2;
382 382
383 int i_n=next[i]; 383 int i_n=next[i];
384 int i_n2=next2[i]; 384 int i_n2=next2[i];
385 385
386 Vector3 point=half_extents; 386 Vector3 point=half_extents;
387 387
388 if (p_normal[i_n]<0) { 388 if (p_normal[i_n]<0) {
389 point[i_n]=-point[i_n]; 389 point[i_n]=-point[i_n];
390 } 390 }
391 if (p_normal[i_n2]<0) { 391 if (p_normal[i_n2]<0) {
392 point[i_n2]=-point[i_n2]; 392 point[i_n2]=-point[i_n2];
393 } 393 }
394 394
395 r_supports[0] = point; 395 r_supports[0] = point;
396 point[i]=-point[i]; 396 point[i]=-point[i];
397 r_supports[1] = point; 397 r_supports[1] = point;
398 return; 398 return;
399 } 399 }
400 } 400 }
401 /* USE POINT */ 401 /* USE POINT */
402 402
403 Vector3 point( 403 Vector3 point(
404 (p_normal.x<0) ? -half_extents.x : half_extents.x, 404 (p_normal.x<0) ? -half_extents.x : half_extents.x,
405 (p_normal.y<0) ? -half_extents.y : half_extents.y, 405 (p_normal.y<0) ? -half_extents.y : half_extents.y,
406 (p_normal.z<0) ? -half_extents.z : half_extents.z 406 (p_normal.z<0) ? -half_extents.z : half_extents.z
407 ); 407 );
408 408
409 r_amount=1; 409 r_amount=1;
410 r_supports[0]=point; 410 r_supports[0]=point;
411} 411}
412 412
413bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 413bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
414 414
415 AABB aabb(-half_extents,half_extents*2.0); 415 AABB aabb(-half_extents,half_extents*2.0);
416 416
417 return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal); 417 return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal);
418 418
419} 419}
420 420
421Vector3 BoxShapeSW::get_moment_of_inertia(float p_mass) const { 421Vector3 BoxShapeSW::get_moment_of_inertia(float p_mass) const {
422 422
423 float lx=half_extents.x; 423 float lx=half_extents.x;
424 float ly=half_extents.y; 424 float ly=half_extents.y;
425 float lz=half_extents.z; 425 float lz=half_extents.z;
426 426
427 return Vector3( (p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) ); 427 return Vector3( (p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) );
428 428
429} 429}
430 430
431void BoxShapeSW::_setup(const Vector3& p_half_extents) { 431void BoxShapeSW::_setup(const Vector3& p_half_extents) {
432 432
433 half_extents=p_half_extents.abs(); 433 half_extents=p_half_extents.abs();
434 434
435 configure(AABB(-half_extents,half_extents*2)); 435 configure(AABB(-half_extents,half_extents*2));
436 436
437 437
438} 438}
439 439
440void BoxShapeSW::set_data(const Variant& p_data) { 440void BoxShapeSW::set_data(const Variant& p_data) {
441 441
442 442
443 _setup(p_data); 443 _setup(p_data);
444} 444}
445 445
446Variant BoxShapeSW::get_data() const { 446Variant BoxShapeSW::get_data() const {
447 447
448 return half_extents; 448 return half_extents;
449} 449}
450 450
451BoxShapeSW::BoxShapeSW() { 451BoxShapeSW::BoxShapeSW() {
452 452
453 453
454} 454}
455 455
456 456
457/********** CAPSULE *************/ 457/********** CAPSULE *************/
458 458
459 459
460void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 460void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
461 461
462 Vector3 n=p_transform.basis.xform_inv(p_normal).normalized(); 462 Vector3 n=p_transform.basis.xform_inv(p_normal).normalized();
463 float h = (n.z > 0) ? height : -height; 463 float h = (n.z > 0) ? height : -height;
464 464
465 n *= radius; 465 n *= radius;
466 n.z += h * 0.5; 466 n.z += h * 0.5;
467 467
468 r_max=p_normal.dot(p_transform.xform(n)); 468 r_max=p_normal.dot(p_transform.xform(n));
469 r_min=p_normal.dot(p_transform.xform(-n)); 469 r_min=p_normal.dot(p_transform.xform(-n));
470 return; 470 return;
471 471
472 n = p_transform.basis.xform(n); 472 n = p_transform.basis.xform(n);
473 473
474 float distance = p_normal.dot( p_transform.origin ); 474 float distance = p_normal.dot( p_transform.origin );
475 float length = Math::abs(p_normal.dot(n)); 475 float length = Math::abs(p_normal.dot(n));
476 r_min = distance - length; 476 r_min = distance - length;
477 r_max = distance + length; 477 r_max = distance + length;
478 478
479 ERR_FAIL_COND( r_max < r_min ); 479 ERR_FAIL_COND( r_max < r_min );
480 480
481} 481}
482 482
483Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const { 483Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const {
484 484
485 Vector3 n=p_normal; 485 Vector3 n=p_normal;
486 486
487 float h = (n.z > 0) ? height : -height; 487 float h = (n.z > 0) ? height : -height;
488 488
489 n*=radius; 489 n*=radius;
490 n.z += h*0.5; 490 n.z += h*0.5;
491 return n; 491 return n;
492} 492}
493 493
494void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { 494void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
495 495
496 496
497 Vector3 n=p_normal; 497 Vector3 n=p_normal;
498 498
499 float d = n.z; 499 float d = n.z;
500 500
501 if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) { 501 if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {
502 502
503 // make it flat 503 // make it flat
504 n.z=0.0; 504 n.z=0.0;
505 n.normalize(); 505 n.normalize();
506 n*=radius; 506 n*=radius;
507 507
508 r_amount=2; 508 r_amount=2;
509 r_supports[0]=n; 509 r_supports[0]=n;
510 r_supports[0].z+=height*0.5; 510 r_supports[0].z+=height*0.5;
511 r_supports[1]=n; 511 r_supports[1]=n;
512 r_supports[1].z-=height*0.5; 512 r_supports[1].z-=height*0.5;
513 513
514 } else { 514 } else {
515 515
516 float h = (d > 0) ? height : -height; 516 float h = (d > 0) ? height : -height;
517 517
518 n*=radius; 518 n*=radius;
519 n.z += h*0.5; 519 n.z += h*0.5;
520 r_amount=1; 520 r_amount=1;
521 *r_supports=n; 521 *r_supports=n;
522 522
523 } 523 }
524 524
525} 525}
526 526
527 527
528bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 528bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
529 529
530 Vector3 norm=(p_end-p_begin).normalized(); 530 Vector3 norm=(p_end-p_begin).normalized();
531 float min_d=1e20; 531 float min_d=1e20;
532 532
533 533
534 Vector3 res,n; 534 Vector3 res,n;
535 bool collision=false; 535 bool collision=false;
536 536
537 Vector3 auxres,auxn; 537 Vector3 auxres,auxn;
538 bool collided; 538 bool collided;
539 539
540 // test against cylinder and spheres :-| 540 // test against cylinder and spheres :-|
541 541
542 collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn); 542 collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn);
543 543
544 if (collided) { 544 if (collided) {
545 float d=norm.dot(auxres); 545 float d=norm.dot(auxres);
546 if (d<min_d) { 546 if (d<min_d) {
547 min_d=d; 547 min_d=d;
548 res=auxres; 548 res=auxres;
549 n=auxn; 549 n=auxn;
550 collision=true; 550 collision=true;
551 } 551 }
552 } 552 }
553 553
554 collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn); 554 collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn);
555 555
556 if (collided) { 556 if (collided) {
557 float d=norm.dot(auxres); 557 float d=norm.dot(auxres);
558 if (d<min_d) { 558 if (d<min_d) {
559 min_d=d; 559 min_d=d;
560 res=auxres; 560 res=auxres;
561 n=auxn; 561 n=auxn;
562 collision=true; 562 collision=true;
563 } 563 }
564 } 564 }
565 565
566 collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn); 566 collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn);
567 567
568 if (collided) { 568 if (collided) {
569 float d=norm.dot(auxres); 569 float d=norm.dot(auxres);
570 570
571 if (d<min_d) { 571 if (d<min_d) {
572 min_d=d; 572 min_d=d;
573 res=auxres; 573 res=auxres;
574 n=auxn; 574 n=auxn;
575 collision=true; 575 collision=true;
576 } 576 }
577 } 577 }
578 578
579 if (collision) { 579 if (collision) {
580 580
581 r_result=res; 581 r_result=res;
582 r_normal=n; 582 r_normal=n;
583 } 583 }
584 return collision; 584 return collision;
585} 585}
586 586
587Vector3 CapsuleShapeSW::get_moment_of_inertia(float p_mass) const { 587Vector3 CapsuleShapeSW::get_moment_of_inertia(float p_mass) const {
588 588
589 // use crappy AABB approximation 589 // use crappy AABB approximation
590 Vector3 extents=get_aabb().size*0.5; 590 Vector3 extents=get_aabb().size*0.5;
591 591
592 return Vector3( 592 return Vector3(
593 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), 593 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
594 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), 594 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
595 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) 595 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
596 ); 596 );
597 597
598} 598}
599 599
600 600
601 601
602 602
603void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius) { 603void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius) {
604 604
605 height=p_height; 605 height=p_height;
606 radius=p_radius; 606 radius=p_radius;
607 configure(AABB(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0))); 607 configure(AABB(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0)));
608 608
609} 609}
610 610
611void CapsuleShapeSW::set_data(const Variant& p_data) { 611void CapsuleShapeSW::set_data(const Variant& p_data) {
612 612
613 Dictionary d = p_data; 613 Dictionary d = p_data;
614 ERR_FAIL_COND(!d.has("radius")); 614 ERR_FAIL_COND(!d.has("radius"));
615 ERR_FAIL_COND(!d.has("height")); 615 ERR_FAIL_COND(!d.has("height"));
616 _setup(d["height"],d["radius"]); 616 _setup(d["height"],d["radius"]);
617 617
618} 618}
619 619
620Variant CapsuleShapeSW::get_data() const { 620Variant CapsuleShapeSW::get_data() const {
621 621
622 Dictionary d; 622 Dictionary d;
623 d["radius"]=radius; 623 d["radius"]=radius;
624 d["height"]=height; 624 d["height"]=height;
625 return d; 625 return d;
626 626
627} 627}
628 628
629 629
630CapsuleShapeSW::CapsuleShapeSW() { 630CapsuleShapeSW::CapsuleShapeSW() {
631 631
632 height=radius=0; 632 height=radius=0;
633 633
634} 634}
635 635
636/********** CONVEX POLYGON *************/ 636/********** CONVEX POLYGON *************/
637 637
638 638
639void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 639void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
640 640
641 641
642 int vertex_count=mesh.vertices.size(); 642 int vertex_count=mesh.vertices.size();
643 if (vertex_count==0) 643 if (vertex_count==0)
644 return; 644 return;
645 645
646 const Vector3 *vrts=&mesh.vertices[0]; 646 const Vector3 *vrts=&mesh.vertices[0];
647 647
648 for (int i=0;i<vertex_count;i++) { 648 for (int i=0;i<vertex_count;i++) {
649 649
650 float d=p_normal.dot( p_transform.xform( vrts[i] ) ); 650 float d=p_normal.dot( p_transform.xform( vrts[i] ) );
651 651
652 if (i==0 || d > r_max) 652 if (i==0 || d > r_max)
653 r_max=d; 653 r_max=d;
654 if (i==0 || d < r_min) 654 if (i==0 || d < r_min)
655 r_min=d; 655 r_min=d;
656 } 656 }
657} 657}
658 658
659Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const { 659Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const {
660 660
661 Vector3 n=p_normal; 661 Vector3 n=p_normal;
662 662
663 int vert_support_idx=-1; 663 int vert_support_idx=-1;
664 float support_max; 664 float support_max;
665 665
666 int vertex_count=mesh.vertices.size(); 666 int vertex_count=mesh.vertices.size();
667 if (vertex_count==0) 667 if (vertex_count==0)
668 return Vector3(); 668 return Vector3();
669 669
670 const Vector3 *vrts=&mesh.vertices[0]; 670 const Vector3 *vrts=&mesh.vertices[0];
671 671
672 for (int i=0;i<vertex_count;i++) { 672 for (int i=0;i<vertex_count;i++) {
673 673
674 float d=n.dot(vrts[i]); 674 float d=n.dot(vrts[i]);
675 675
676 if (i==0 || d > support_max) { 676 if (i==0 || d > support_max) {
677 support_max=d; 677 support_max=d;
678 vert_support_idx=i; 678 vert_support_idx=i;
679 } 679 }
680 } 680 }
681 681
682 return vrts[vert_support_idx]; 682 return vrts[vert_support_idx];
683 683
684} 684}
685 685
686 686
687 687
688void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { 688void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
689 689
690 const Geometry::MeshData::Face *faces = mesh.faces.ptr(); 690 const Geometry::MeshData::Face *faces = mesh.faces.ptr();
691 int fc = mesh.faces.size(); 691 int fc = mesh.faces.size();
692 692
693 const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); 693 const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
694 int ec = mesh.edges.size(); 694 int ec = mesh.edges.size();
695 695
696 const Vector3 *vertices = mesh.vertices.ptr(); 696 const Vector3 *vertices = mesh.vertices.ptr();
697 int vc = mesh.vertices.size(); 697 int vc = mesh.vertices.size();
698 698
699 //find vertex first 699 //find vertex first
700 real_t max; 700 real_t max;
701 int vtx; 701 int vtx;
702 702
703 for (int i=0;i<vc;i++) { 703 for (int i=0;i<vc;i++) {
704 704
705 float d=p_normal.dot(vertices[i]); 705 float d=p_normal.dot(vertices[i]);
706 706
707 if (i==0 || d > max) { 707 if (i==0 || d > max) {
708 max=d; 708 max=d;
709 vtx=i; 709 vtx=i;
710 } 710 }
711 } 711 }
712 712
713 713
714 for(int i=0;i<fc;i++) { 714 for(int i=0;i<fc;i++) {
715 715
716 if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) { 716 if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) {
717 717
718 int ic = faces[i].indices.size(); 718 int ic = faces[i].indices.size();
719 const int *ind=faces[i].indices.ptr(); 719 const int *ind=faces[i].indices.ptr();
720 720
721 bool valid=false; 721 bool valid=false;
722 for(int j=0;j<ic;j++) { 722 for(int j=0;j<ic;j++) {
723 if (ind[j]==vtx) { 723 if (ind[j]==vtx) {
724 valid=true; 724 valid=true;
725 break; 725 break;
726 } 726 }
727 } 727 }
728 728
729 if (!valid) 729 if (!valid)
730 continue; 730 continue;
731 731
732 int m = MIN(p_max,ic); 732 int m = MIN(p_max,ic);
733 for(int j=0;j<m;j++) { 733 for(int j=0;j<m;j++) {
734 734
735 r_supports[j]=vertices[ind[j]]; 735 r_supports[j]=vertices[ind[j]];
736 } 736 }
737 r_amount=m; 737 r_amount=m;
738 return; 738 return;
739 } 739 }
740 } 740 }
741 741
742 for(int i=0;i<ec;i++) { 742 for(int i=0;i<ec;i++) {
743 743
744 744
745 float dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal); 745 float dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal);
746 dot=ABS(dot); 746 dot=ABS(dot);
747 if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) { 747 if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) {
748 748
749 r_amount=2; 749 r_amount=2;
750 r_supports[0]=vertices[edges[i].a]; 750 r_supports[0]=vertices[edges[i].a];
751 r_supports[1]=vertices[edges[i].b]; 751 r_supports[1]=vertices[edges[i].b];
752 return; 752 return;
753 } 753 }
754 } 754 }
755 755
756 756
757 r_supports[0]=vertices[vtx]; 757 r_supports[0]=vertices[vtx];
758 r_amount=1; 758 r_amount=1;
759} 759}
760 760
761bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 761bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
762 762
763 763
764 764
765 const Geometry::MeshData::Face *faces = mesh.faces.ptr(); 765 const Geometry::MeshData::Face *faces = mesh.faces.ptr();
766 int fc = mesh.faces.size(); 766 int fc = mesh.faces.size();
767 767
768 const Vector3 *vertices = mesh.vertices.ptr(); 768 const Vector3 *vertices = mesh.vertices.ptr();
769 int vc = mesh.vertices.size(); 769 int vc = mesh.vertices.size();
770 770
771 Vector3 n = p_end-p_begin; 771 Vector3 n = p_end-p_begin;
772 float min = 1e20; 772 float min = 1e20;
773 bool col=false; 773 bool col=false;
774 774
775 for(int i=0;i<fc;i++) { 775 for(int i=0;i<fc;i++) {
776 776
777 if (faces[i].plane.normal.dot(n) > 0) 777 if (faces[i].plane.normal.dot(n) > 0)
778 continue; //opposing face 778 continue; //opposing face
779 779
780 int ic = faces[i].indices.size(); 780 int ic = faces[i].indices.size();
781 const int *ind=faces[i].indices.ptr(); 781 const int *ind=faces[i].indices.ptr();
782 782
783 for(int j=1;j<ic-1;j++) { 783 for(int j=1;j<ic-1;j++) {
784 784
785 Face3 f(vertices[ind[0]],vertices[ind[i]],vertices[ind[i+1]]); 785 Face3 f(vertices[ind[0]],vertices[ind[j]],vertices[ind[j+1]]);
786 Vector3 result; 786 Vector3 result;
787 if (f.intersects_segment(p_begin,p_end,&result)) { 787 if (f.intersects_segment(p_begin,p_end,&result)) {
788 float d = n.dot(result); 788 float d = n.dot(result);
789 if (d<min) { 789 if (d<min) {
790 min=d; 790 min=d;
791 r_result=result; 791 r_result=result;
792 r_normal=faces[i].plane.normal; 792 r_normal=faces[i].plane.normal;
793 col=true; 793 col=true;
794 } 794 }
795 795
796 break; 796 break;
797 } 797 }
798 798
799 } 799 }
800 } 800 }
801 801
802 return col; 802 return col;
803 803
804} 804}
805 805
806Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(float p_mass) const { 806Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(float p_mass) const {
807 807
808 // use crappy AABB approximation 808 // use crappy AABB approximation
809 Vector3 extents=get_aabb().size*0.5; 809 Vector3 extents=get_aabb().size*0.5;
810 810
811 return Vector3( 811 return Vector3(
812 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), 812 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
813 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), 813 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
814 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) 814 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
815 ); 815 );
816 816
817} 817}
818 818
819void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices) { 819void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices) {
820 820
821 Error err = QuickHull::build(p_vertices,mesh); 821 Error err = QuickHull::build(p_vertices,mesh);
822 AABB _aabb; 822 AABB _aabb;
823 823
824 for(int i=0;i<mesh.vertices.size();i++) { 824 for(int i=0;i<mesh.vertices.size();i++) {
825 825
826 if (i==0) 826 if (i==0)
827 _aabb.pos=mesh.vertices[i]; 827 _aabb.pos=mesh.vertices[i];
828 else 828 else
829 _aabb.expand_to(mesh.vertices[i]); 829 _aabb.expand_to(mesh.vertices[i]);
830 } 830 }
831 831
832 configure(_aabb); 832 configure(_aabb);
833 833
834 834
835} 835}
836 836
837void ConvexPolygonShapeSW::set_data(const Variant& p_data) { 837void ConvexPolygonShapeSW::set_data(const Variant& p_data) {
838 838
839 _setup(p_data); 839 _setup(p_data);
840 840
841} 841}
842 842
843Variant ConvexPolygonShapeSW::get_data() const { 843Variant ConvexPolygonShapeSW::get_data() const {
844 844
845 return mesh.vertices; 845 return mesh.vertices;
846} 846}
847 847
848 848
849ConvexPolygonShapeSW::ConvexPolygonShapeSW() { 849ConvexPolygonShapeSW::ConvexPolygonShapeSW() {
850 850
851 851
852} 852}
853 853
854 854
855/********** FACE POLYGON *************/ 855/********** FACE POLYGON *************/
856 856
857 857
858void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 858void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
859 859
860 for (int i=0;i<3;i++) { 860 for (int i=0;i<3;i++) {
861 861
862 Vector3 v=p_transform.xform(vertex[i]); 862 Vector3 v=p_transform.xform(vertex[i]);
863 float d=p_normal.dot(v); 863 float d=p_normal.dot(v);
864 864
865 if (i==0 || d > r_max) 865 if (i==0 || d > r_max)
866 r_max=d; 866 r_max=d;
867 867
868 if (i==0 || d < r_min) 868 if (i==0 || d < r_min)
869 r_min=d; 869 r_min=d;
870 } 870 }
871} 871}
872 872
873Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const { 873Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const {
874 874
875 875
876 Vector3 n=p_normal; 876 Vector3 n=p_normal;
877 877
878 int vert_support_idx=-1; 878 int vert_support_idx=-1;
879 float support_max; 879 float support_max;
880 880
881 for (int i=0;i<3;i++) { 881 for (int i=0;i<3;i++) {
882 882
883 //float d=n.dot(vertex[i]); 883 //float d=n.dot(vertex[i]);
884 float d=p_normal.dot(vertex[i]); 884 float d=p_normal.dot(vertex[i]);
885 885
886 if (i==0 || d > support_max) { 886 if (i==0 || d > support_max) {
887 support_max=d; 887 support_max=d;
888 vert_support_idx=i; 888 vert_support_idx=i;
889 } 889 }
890 } 890 }
891 891
892 return vertex[vert_support_idx]; 892 return vertex[vert_support_idx];
893} 893}
894 894
895void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { 895void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
896 896
897 Vector3 n=p_normal; 897 Vector3 n=p_normal;
898 898
899 /** TEST FACE AS SUPPORT **/ 899 /** TEST FACE AS SUPPORT **/
900 if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { 900 if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {
901 901
902 r_amount=3; 902 r_amount=3;
903 for (int i=0;i<3;i++) { 903 for (int i=0;i<3;i++) {
904 904
905 r_supports[i]=vertex[i]; 905 r_supports[i]=vertex[i];
906 } 906 }
907 return; 907 return;
908 908
909 } 909 }
910 910
911 /** FIND SUPPORT VERTEX **/ 911 /** FIND SUPPORT VERTEX **/
912 912
913 int vert_support_idx=-1; 913 int vert_support_idx=-1;
914 float support_max; 914 float support_max;
915 915
916 for (int i=0;i<3;i++) { 916 for (int i=0;i<3;i++) {
917 917
918 float d=n.dot(vertex[i]); 918 float d=n.dot(vertex[i]);
919 919
920 if (i==0 || d > support_max) { 920 if (i==0 || d > support_max) {
921 support_max=d; 921 support_max=d;
922 vert_support_idx=i; 922 vert_support_idx=i;
923 } 923 }
924 } 924 }
925 925
926 /** TEST EDGES AS SUPPORT **/ 926 /** TEST EDGES AS SUPPORT **/
927 927
928 for (int i=0;i<3;i++) { 928 for (int i=0;i<3;i++) {
929 929
930 int nx=(i+1)%3; 930 int nx=(i+1)%3;
931 //if (i!=vert_support_idx && nx!=vert_support_idx) 931 //if (i!=vert_support_idx && nx!=vert_support_idx)
932 // continue; 932 // continue;
933 933
934 // check if edge is valid as a support 934 // check if edge is valid as a support
935 float dot=(vertex[i]-vertex[nx]).normalized().dot(n); 935 float dot=(vertex[i]-vertex[nx]).normalized().dot(n);
936 dot=ABS(dot); 936 dot=ABS(dot);
937 if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { 937 if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
938 938
939 r_amount=2; 939 r_amount=2;
940 r_supports[0]=vertex[i]; 940 r_supports[0]=vertex[i];
941 r_supports[1]=vertex[nx]; 941 r_supports[1]=vertex[nx];
942 return; 942 return;
943 } 943 }
944 } 944 }
945 945
946 r_amount=1; 946 r_amount=1;
947 r_supports[0]=vertex[vert_support_idx]; 947 r_supports[0]=vertex[vert_support_idx];
948} 948}
949 949
950bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 950bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
951 951
952 952
953 bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result); 953 bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result);
954 if (c) 954 if (c)
955 r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal; 955 r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal;
956 956
957 return c; 957 return c;
958} 958}
959 959
960Vector3 FaceShapeSW::get_moment_of_inertia(float p_mass) const { 960Vector3 FaceShapeSW::get_moment_of_inertia(float p_mass) const {
961 961
962 return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! 962 return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
963 963
964} 964}
965 965
966FaceShapeSW::FaceShapeSW() { 966FaceShapeSW::FaceShapeSW() {
967 967
968 configure(AABB()); 968 configure(AABB());
969 969
970} 970}
971 971
972 972
973 973
974DVector<Vector3> ConcavePolygonShapeSW::get_faces() const { 974DVector<Vector3> ConcavePolygonShapeSW::get_faces() const {
975 975
976 976
977 DVector<Vector3> rfaces; 977 DVector<Vector3> rfaces;
978 rfaces.resize(faces.size()*3); 978 rfaces.resize(faces.size()*3);
979 979
980 for(int i=0;i<faces.size();i++) { 980 for(int i=0;i<faces.size();i++) {
981 981
982 Face f=faces.get(i); 982 Face f=faces.get(i);
983 983
984 for(int j=0;j<3;j++) { 984 for(int j=0;j<3;j++) {
985 985
986 rfaces.set(i*3+j, vertices.get( f.indices[j] ) ); 986 rfaces.set(i*3+j, vertices.get( f.indices[j] ) );
987 } 987 }
988 } 988 }
989 989
990 return rfaces; 990 return rfaces;
991} 991}
992 992
993void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 993void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
994 994
995 int count=vertices.size(); 995 int count=vertices.size();
996 DVector<Vector3>::Read r=vertices.read(); 996 DVector<Vector3>::Read r=vertices.read();
997 const Vector3 *vptr=r.ptr(); 997 const Vector3 *vptr=r.ptr();
998 998
999 for (int i=0;i<count;i++) { 999 for (int i=0;i<count;i++) {
1000 1000
1001 float d=p_normal.dot( p_transform.xform( vptr[i] ) ); 1001 float d=p_normal.dot( p_transform.xform( vptr[i] ) );
1002 1002
1003 if (i==0 || d > r_max) 1003 if (i==0 || d > r_max)
1004 r_max=d; 1004 r_max=d;
1005 if (i==0 || d < r_min) 1005 if (i==0 || d < r_min)
1006 r_min=d; 1006 r_min=d;
1007 1007
1008 } 1008 }
1009} 1009}
1010 1010
1011Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const { 1011Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const {
1012 1012
1013 1013
1014 int count=vertices.size(); 1014 int count=vertices.size();
1015 DVector<Vector3>::Read r=vertices.read(); 1015 DVector<Vector3>::Read r=vertices.read();
1016 const Vector3 *vptr=r.ptr(); 1016 const Vector3 *vptr=r.ptr();
1017 1017
1018 Vector3 n=p_normal; 1018 Vector3 n=p_normal;
1019 1019
1020 int vert_support_idx=-1; 1020 int vert_support_idx=-1;
1021 float support_max; 1021 float support_max;
1022 1022
1023 for (int i=0;i<count;i++) { 1023 for (int i=0;i<count;i++) {
1024 1024
1025 float d=n.dot(vptr[i]); 1025 float d=n.dot(vptr[i]);
1026 1026
1027 if (i==0 || d > support_max) { 1027 if (i==0 || d > support_max) {
1028 support_max=d; 1028 support_max=d;
1029 vert_support_idx=i; 1029 vert_support_idx=i;
1030 } 1030 }
1031 } 1031 }
1032 1032
1033 1033
1034 return vptr[vert_support_idx]; 1034 return vptr[vert_support_idx];
1035 1035
1036} 1036}
1037 1037
1038void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const { 1038void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const {
1039 1039
1040 const BVH *bvh=&p_params->bvh[p_idx]; 1040 const BVH *bvh=&p_params->bvh[p_idx];
1041 1041
1042 1042
1043 //if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d) 1043 //if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
1044 // return; //test against whole AABB, which isn't very costly 1044 // return; //test against whole AABB, which isn't very costly
1045 1045
1046 1046
1047 //printf("addr: %p\n",bvh); 1047 //printf("addr: %p\n",bvh);
1048 if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) { 1048 if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) {
1049 1049
1050 return; 1050 return;
1051 } 1051 }
1052 1052
1053 1053
1054 if (bvh->face_index>=0) { 1054 if (bvh->face_index>=0) {
1055 1055
1056 1056
1057 Vector3 res; 1057 Vector3 res;
1058 Vector3 vertices[3]={ 1058 Vector3 vertices[3]={
1059 p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ], 1059 p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ],
1060 p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ], 1060 p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ],
1061 p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ] 1061 p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ]
1062 }; 1062 };
1063 1063
1064 if (Geometry::segment_intersects_triangle( 1064 if (Geometry::segment_intersects_triangle(
1065 p_params->from, 1065 p_params->from,
1066 p_params->to, 1066 p_params->to,
1067 vertices[0], 1067 vertices[0],
1068 vertices[1], 1068 vertices[1],
1069 vertices[2], 1069 vertices[2],
1070 &res)) { 1070 &res)) {
1071 1071
1072 1072
1073 float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from); 1073 float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from);
1074 //TODO, seems segmen/triangle intersection is broken :( 1074 //TODO, seems segmen/triangle intersection is broken :(
1075 if (d>0 && d<p_params->min_d) { 1075 if (d>0 && d<p_params->min_d) {
1076 1076
1077 p_params->min_d=d; 1077 p_params->min_d=d;
1078 p_params->result=res; 1078 p_params->result=res;
1079 p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal; 1079 p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal;
1080 p_params->collisions++; 1080 p_params->collisions++;
1081 } 1081 }
1082 1082
1083 } 1083 }
1084 1084
1085 1085
1086 1086
1087 } else { 1087 } else {
1088 1088
1089 if (bvh->left>=0) 1089 if (bvh->left>=0)
1090 _cull_segment(bvh->left,p_params); 1090 _cull_segment(bvh->left,p_params);
1091 if (bvh->right>=0) 1091 if (bvh->right>=0)
1092 _cull_segment(bvh->right,p_params); 1092 _cull_segment(bvh->right,p_params);
1093 1093
1094 1094
1095 } 1095 }
1096} 1096}
1097 1097
1098bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { 1098bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
1099 1099
1100 // unlock data 1100 // unlock data
1101 DVector<Face>::Read fr=faces.read(); 1101 DVector<Face>::Read fr=faces.read();
1102 DVector<Vector3>::Read vr=vertices.read(); 1102 DVector<Vector3>::Read vr=vertices.read();
1103 DVector<BVH>::Read br=bvh.read(); 1103 DVector<BVH>::Read br=bvh.read();
1104 1104
1105 1105
1106 _SegmentCullParams params; 1106 _SegmentCullParams params;
1107 params.from=p_begin; 1107 params.from=p_begin;
1108 params.to=p_end; 1108 params.to=p_end;
1109 params.collisions=0; 1109 params.collisions=0;
1110 params.normal=(p_end-p_begin).normalized(); 1110 params.normal=(p_end-p_begin).normalized();
1111 1111
1112 params.faces=fr.ptr(); 1112 params.faces=fr.ptr();
1113 params.vertices=vr.ptr(); 1113 params.vertices=vr.ptr();
1114 params.bvh=br.ptr(); 1114 params.bvh=br.ptr();
1115 1115
1116 params.min_d=1e20; 1116 params.min_d=1e20;
1117 // cull 1117 // cull
1118 _cull_segment(0,&params); 1118 _cull_segment(0,&params);
1119 1119
1120 if (params.collisions>0) { 1120 if (params.collisions>0) {
1121 1121
1122 1122
1123 r_result=params.result; 1123 r_result=params.result;
1124 r_normal=params.normal; 1124 r_normal=params.normal;
1125 return true; 1125 return true;
1126 } else { 1126 } else {
1127 1127
1128 return false; 1128 return false;
1129 } 1129 }
1130} 1130}
1131 1131
1132void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const { 1132void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const {
1133 1133
1134 const BVH* bvh=&p_params->bvh[p_idx]; 1134 const BVH* bvh=&p_params->bvh[p_idx];
1135 1135
1136 if (!p_params->aabb.intersects( bvh->aabb )) 1136 if (!p_params->aabb.intersects( bvh->aabb ))
1137 return; 1137 return;
1138 1138
1139 if (bvh->face_index>=0) { 1139 if (bvh->face_index>=0) {
1140 1140
1141 const Face *f=&p_params->faces[ bvh->face_index ]; 1141 const Face *f=&p_params->faces[ bvh->face_index ];
1142 FaceShapeSW *face=p_params->face; 1142 FaceShapeSW *face=p_params->face;
1143 face->normal=f->normal; 1143 face->normal=f->normal;
1144 face->vertex[0]=p_params->vertices[f->indices[0]]; 1144 face->vertex[0]=p_params->vertices[f->indices[0]];
1145 face->vertex[1]=p_params->vertices[f->indices[1]]; 1145 face->vertex[1]=p_params->vertices[f->indices[1]];
1146 face->vertex[2]=p_params->vertices[f->indices[2]]; 1146 face->vertex[2]=p_params->vertices[f->indices[2]];
1147 p_params->callback(p_params->userdata,face); 1147 p_params->callback(p_params->userdata,face);
1148 1148
1149 } else { 1149 } else {
1150 1150
1151 if (bvh->left>=0) { 1151 if (bvh->left>=0) {
1152 1152
1153 _cull(bvh->left,p_params); 1153 _cull(bvh->left,p_params);
1154 1154
1155 } 1155 }
1156 1156
1157 if (bvh->right>=0) { 1157 if (bvh->right>=0) {
1158 1158
1159 _cull(bvh->right,p_params); 1159 _cull(bvh->right,p_params);
1160 } 1160 }
1161 1161
1162 } 1162 }
1163} 1163}
1164 1164
1165void ConcavePolygonShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const { 1165void ConcavePolygonShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
1166 1166
1167 // make matrix local to concave 1167 // make matrix local to concave
1168 1168
1169 AABB local_aabb=p_local_aabb; 1169 AABB local_aabb=p_local_aabb;
1170 1170
1171 // unlock data 1171 // unlock data
1172 DVector<Face>::Read fr=faces.read(); 1172 DVector<Face>::Read fr=faces.read();
1173 DVector<Vector3>::Read vr=vertices.read(); 1173 DVector<Vector3>::Read vr=vertices.read();
1174 DVector<BVH>::Read br=bvh.read(); 1174 DVector<BVH>::Read br=bvh.read();
1175 1175
1176 FaceShapeSW face; // use this to send in the callback 1176 FaceShapeSW face; // use this to send in the callback
1177 1177
1178 _CullParams params; 1178 _CullParams params;
1179 params.aabb=local_aabb; 1179 params.aabb=local_aabb;
1180 params.face=&face; 1180 params.face=&face;
1181 params.faces=fr.ptr(); 1181 params.faces=fr.ptr();
1182 params.vertices=vr.ptr(); 1182 params.vertices=vr.ptr();
1183 params.bvh=br.ptr(); 1183 params.bvh=br.ptr();
1184 params.callback=p_callback; 1184 params.callback=p_callback;
1185 params.userdata=p_userdata; 1185 params.userdata=p_userdata;
1186 1186
1187 // cull 1187 // cull
1188 _cull(0,&params); 1188 _cull(0,&params);
1189 1189
1190} 1190}
1191 1191
1192Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(float p_mass) const { 1192Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(float p_mass) const {
1193 1193
1194 // use crappy AABB approximation 1194 // use crappy AABB approximation
1195 Vector3 extents=get_aabb().size*0.5; 1195 Vector3 extents=get_aabb().size*0.5;
1196 1196
1197 return Vector3( 1197 return Vector3(
1198 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), 1198 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
1199 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), 1199 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
1200 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) 1200 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
1201 ); 1201 );
1202} 1202}
1203 1203
1204 1204
1205struct _VolumeSW_BVH_Element { 1205struct _VolumeSW_BVH_Element {
1206 1206
1207 AABB aabb; 1207 AABB aabb;
1208 Vector3 center; 1208 Vector3 center;
1209 int face_index; 1209 int face_index;
1210}; 1210};
1211 1211
1212struct _VolumeSW_BVH_CompareX { 1212struct _VolumeSW_BVH_CompareX {
1213 1213
1214 _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const { 1214 _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
1215 1215
1216 return a.center.x<b.center.x; 1216 return a.center.x<b.center.x;
1217 } 1217 }
1218}; 1218};
1219 1219
1220 1220
1221struct _VolumeSW_BVH_CompareY { 1221struct _VolumeSW_BVH_CompareY {
1222 1222
1223 _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const { 1223 _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
1224 1224
1225 return a.center.y<b.center.y; 1225 return a.center.y<b.center.y;
1226 } 1226 }
1227}; 1227};
1228 1228
1229struct _VolumeSW_BVH_CompareZ { 1229struct _VolumeSW_BVH_CompareZ {
1230 1230
1231 _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const { 1231 _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
1232 1232
1233 return a.center.z<b.center.z; 1233 return a.center.z<b.center.z;
1234 } 1234 }
1235}; 1235};
1236 1236
1237struct _VolumeSW_BVH { 1237struct _VolumeSW_BVH {
1238 1238
1239 AABB aabb; 1239 AABB aabb;
1240 _VolumeSW_BVH *left; 1240 _VolumeSW_BVH *left;
1241 _VolumeSW_BVH *right; 1241 _VolumeSW_BVH *right;
1242 1242
1243 int face_index; 1243 int face_index;
1244}; 1244};
1245 1245
1246 1246
1247_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) { 1247_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) {
1248 1248
1249 _VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH ); 1249 _VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH );
1250 1250
1251 if (p_size==1) { 1251 if (p_size==1) {
1252 //leaf 1252 //leaf
1253 bvh->aabb=p_elements[0].aabb; 1253 bvh->aabb=p_elements[0].aabb;
1254 bvh->left=NULL; 1254 bvh->left=NULL;
1255 bvh->right=NULL; 1255 bvh->right=NULL;
1256 bvh->face_index=p_elements->face_index; 1256 bvh->face_index=p_elements->face_index;
1257 count++; 1257 count++;
1258 return bvh; 1258 return bvh;
1259 } else { 1259 } else {
1260 1260
1261 bvh->face_index=-1; 1261 bvh->face_index=-1;
1262 } 1262 }
1263 1263
1264 AABB aabb; 1264 AABB aabb;
1265 for(int i=0;i<p_size;i++) { 1265 for(int i=0;i<p_size;i++) {
1266 1266
1267 if (i==0) 1267 if (i==0)
1268 aabb=p_elements[i].aabb; 1268 aabb=p_elements[i].aabb;
1269 else 1269 else
1270 aabb.merge_with(p_elements[i].aabb); 1270 aabb.merge_with(p_elements[i].aabb);
1271 } 1271 }
1272 bvh->aabb=aabb; 1272 bvh->aabb=aabb;
1273 switch(aabb.get_longest_axis_index()) { 1273 switch(aabb.get_longest_axis_index()) {
1274 1274
1275 case 0: { 1275 case 0: {
1276 1276
1277 SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x; 1277 SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x;
1278 sort_x.sort(p_elements,p_size); 1278 sort_x.sort(p_elements,p_size);
1279 1279
1280 } break; 1280 } break;
1281 case 1: { 1281 case 1: {
1282 1282
1283 SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y; 1283 SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y;
1284 sort_y.sort(p_elements,p_size); 1284 sort_y.sort(p_elements,p_size);
1285 } break; 1285 } break;
1286 case 2: { 1286 case 2: {
1287 1287
1288 SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z; 1288 SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z;
1289 sort_z.sort(p_elements,p_size); 1289 sort_z.sort(p_elements,p_size);
1290 } break; 1290 } break;
1291 } 1291 }
1292 1292
1293 int split=p_size/2; 1293 int split=p_size/2;
1294 bvh->left=_volume_sw_build_bvh(p_elements,split,count); 1294 bvh->left=_volume_sw_build_bvh(p_elements,split,count);
1295 bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count); 1295 bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count);
1296 1296
1297// printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index); 1297// printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
1298 count++; 1298 count++;
1299 return bvh; 1299 return bvh;
1300} 1300}
1301 1301
1302 1302
1303void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) { 1303void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) {
1304 1304
1305 int idx=p_idx; 1305 int idx=p_idx;
1306 1306
1307 1307
1308 p_bvh_array[idx].aabb=p_bvh_tree->aabb; 1308 p_bvh_array[idx].aabb=p_bvh_tree->aabb;
1309 p_bvh_array[idx].face_index=p_bvh_tree->face_index; 1309 p_bvh_array[idx].face_index=p_bvh_tree->face_index;
1310// printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right); 1310// printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
1311 1311
1312 1312
1313 if (p_bvh_tree->left) { 1313 if (p_bvh_tree->left) {
1314 p_bvh_array[idx].left=++p_idx; 1314 p_bvh_array[idx].left=++p_idx;
1315 _fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx); 1315 _fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx);
1316 1316
1317 } else { 1317 } else {
1318 1318
1319 p_bvh_array[p_idx].left=-1; 1319 p_bvh_array[p_idx].left=-1;
1320 } 1320 }
1321 1321
1322 if (p_bvh_tree->right) { 1322 if (p_bvh_tree->right) {
1323 p_bvh_array[idx].right=++p_idx; 1323 p_bvh_array[idx].right=++p_idx;
1324 _fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx); 1324 _fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx);
1325 1325
1326 } else { 1326 } else {
1327 1327
1328 p_bvh_array[p_idx].right=-1; 1328 p_bvh_array[p_idx].right=-1;
1329 } 1329 }
1330 1330
1331 memdelete(p_bvh_tree); 1331 memdelete(p_bvh_tree);
1332 1332
1333} 1333}
1334 1334
1335void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) { 1335void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) {
1336 1336
1337 int src_face_count=p_faces.size(); 1337 int src_face_count=p_faces.size();
1338 ERR_FAIL_COND(src_face_count%3); 1338 ERR_FAIL_COND(src_face_count%3);
1339 src_face_count/=3; 1339 src_face_count/=3;
1340 1340
1341 DVector<Vector3>::Read r = p_faces.read(); 1341 DVector<Vector3>::Read r = p_faces.read();
1342 const Vector3 * facesr= r.ptr(); 1342 const Vector3 * facesr= r.ptr();
1343 1343
1344#if 0 1344#if 0
1345 Map<Vector3,int> point_map; 1345 Map<Vector3,int> point_map;
1346 List<Face> face_list; 1346 List<Face> face_list;
1347 1347
1348 1348
1349 for(int i=0;i<src_face_count;i++) { 1349 for(int i=0;i<src_face_count;i++) {
1350 1350
1351 Face3 faceaux; 1351 Face3 faceaux;
1352 1352
1353 for(int j=0;j<3;j++) { 1353 for(int j=0;j<3;j++) {
1354 1354
1355 faceaux.vertex[j]=facesr[i*3+j].snapped(_POINT_SNAP); 1355 faceaux.vertex[j]=facesr[i*3+j].snapped(_POINT_SNAP);
1356 //faceaux.vertex[j]=facesr[i*3+j];//facesr[i*3+j].snapped(_POINT_SNAP); 1356 //faceaux.vertex[j]=facesr[i*3+j];//facesr[i*3+j].snapped(_POINT_SNAP);
1357 } 1357 }
1358 1358
1359 ERR_CONTINUE( faceaux.is_degenerate() ); 1359 ERR_CONTINUE( faceaux.is_degenerate() );
1360 1360
1361 Face face; 1361 Face face;
1362 1362
1363 for(int j=0;j<3;j++) { 1363 for(int j=0;j<3;j++) {
1364 1364
1365 1365
1366 Map<Vector3,int>::Element *E=point_map.find(faceaux.vertex[j]); 1366 Map<Vector3,int>::Element *E=point_map.find(faceaux.vertex[j]);
1367 if (E) { 1367 if (E) {
1368 1368
1369 face.indices[j]=E->value(); 1369 face.indices[j]=E->value();
1370 } else { 1370 } else {
1371 1371
1372 face.indices[j]=point_map.size(); 1372 face.indices[j]=point_map.size();
1373 point_map.insert(faceaux.vertex[j],point_map.size()); 1373 point_map.insert(faceaux.vertex[j],point_map.size());
1374 1374
1375 } 1375 }
1376 } 1376 }
1377 1377
1378 face_list.push_back(face); 1378 face_list.push_back(face);
1379 } 1379 }
1380 1380
1381 vertices.resize( point_map.size() ); 1381 vertices.resize( point_map.size() );
1382 1382
1383 DVector<Vector3>::Write vw = vertices.write(); 1383 DVector<Vector3>::Write vw = vertices.write();
1384 Vector3 *verticesw=vw.ptr(); 1384 Vector3 *verticesw=vw.ptr();
1385 1385
1386 AABB _aabb; 1386 AABB _aabb;
1387 1387
1388 for( Map<Vector3,int>::Element *E=point_map.front();E;E=E->next()) { 1388 for( Map<Vector3,int>::Element *E=point_map.front();E;E=E->next()) {
1389 1389
1390 if (E==point_map.front()) { 1390 if (E==point_map.front()) {
1391 _aabb.pos=E->key(); 1391 _aabb.pos=E->key();
1392 } else { 1392 } else {
1393 1393
1394 _aabb.expand_to(E->key()); 1394 _aabb.expand_to(E->key());
1395 } 1395 }
1396 verticesw[E->value()]=E->key(); 1396 verticesw[E->value()]=E->key();
1397 } 1397 }
1398 1398
1399 point_map.clear(); // not needed anymore 1399 point_map.clear(); // not needed anymore
1400 1400
1401 faces.resize(face_list.size()); 1401 faces.resize(face_list.size());
1402 DVector<Face>::Write w = faces.write(); 1402 DVector<Face>::Write w = faces.write();
1403 Face *facesw=w.ptr(); 1403 Face *facesw=w.ptr();
1404 1404
1405 int fc=0; 1405 int fc=0;
1406 1406
1407 for( List<Face>::Element *E=face_list.front();E;E=E->next()) { 1407 for( List<Face>::Element *E=face_list.front();E;E=E->next()) {
1408 1408
1409 facesw[fc++]=E->get(); 1409 facesw[fc++]=E->get();
1410 } 1410 }
1411 1411
1412 face_list.clear(); 1412 face_list.clear();
1413 1413
1414 1414
1415 DVector<_VolumeSW_BVH_Element> bvh_array; 1415 DVector<_VolumeSW_BVH_Element> bvh_array;
1416 bvh_array.resize( fc ); 1416 bvh_array.resize( fc );
1417 1417
1418 DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write(); 1418 DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
1419 _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr(); 1419 _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
1420 1420
1421 1421
1422 for(int i=0;i<fc;i++) { 1422 for(int i=0;i<fc;i++) {
1423 1423
1424 AABB face_aabb; 1424 AABB face_aabb;
1425 face_aabb.pos=verticesw[facesw[i].indices[0]]; 1425 face_aabb.pos=verticesw[facesw[i].indices[0]];
1426 face_aabb.expand_to( verticesw[facesw[i].indices[1]] ); 1426 face_aabb.expand_to( verticesw[facesw[i].indices[1]] );
1427 face_aabb.expand_to( verticesw[facesw[i].indices[2]] ); 1427 face_aabb.expand_to( verticesw[facesw[i].indices[2]] );
1428 1428
1429 bvh_arrayw[i].face_index=i; 1429 bvh_arrayw[i].face_index=i;
1430 bvh_arrayw[i].aabb=face_aabb; 1430 bvh_arrayw[i].aabb=face_aabb;
1431 bvh_arrayw[i].center=face_aabb.pos+face_aabb.size*0.5; 1431 bvh_arrayw[i].center=face_aabb.pos+face_aabb.size*0.5;
1432 1432
1433 } 1433 }
1434 1434
1435 w=DVector<Face>::Write(); 1435 w=DVector<Face>::Write();
1436 vw=DVector<Vector3>::Write(); 1436 vw=DVector<Vector3>::Write();
1437 1437
1438 1438
1439 int count=0; 1439 int count=0;
1440 _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,fc,count); 1440 _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,fc,count);
1441 1441
1442 ERR_FAIL_COND(count==0); 1442 ERR_FAIL_COND(count==0);
1443 1443
1444 bvhw=DVector<_VolumeSW_BVH_Element>::Write(); 1444 bvhw=DVector<_VolumeSW_BVH_Element>::Write();
1445 1445
1446 bvh.resize( count+1 ); 1446 bvh.resize( count+1 );
1447 1447
1448 DVector<BVH>::Write bvhw2 = bvh.write(); 1448 DVector<BVH>::Write bvhw2 = bvh.write();
1449 BVH*bvh_arrayw2=bvhw2.ptr(); 1449 BVH*bvh_arrayw2=bvhw2.ptr();
1450 1450
1451 int idx=0; 1451 int idx=0;
1452 _fill_bvh(bvh_tree,bvh_arrayw2,idx); 1452 _fill_bvh(bvh_tree,bvh_arrayw2,idx);
1453 1453
1454 set_aabb(_aabb); 1454 set_aabb(_aabb);
1455 1455
1456#else 1456#else
1457 DVector<_VolumeSW_BVH_Element> bvh_array; 1457 DVector<_VolumeSW_BVH_Element> bvh_array;
1458 bvh_array.resize( src_face_count ); 1458 bvh_array.resize( src_face_count );
1459 1459
1460 DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write(); 1460 DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
1461 _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr(); 1461 _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
1462 1462
1463 faces.resize(src_face_count); 1463 faces.resize(src_face_count);
1464 DVector<Face>::Write w = faces.write(); 1464 DVector<Face>::Write w = faces.write();
1465 Face *facesw=w.ptr(); 1465 Face *facesw=w.ptr();
1466 1466
1467 vertices.resize( src_face_count*3 ); 1467 vertices.resize( src_face_count*3 );
1468 1468
1469 DVector<Vector3>::Write vw = vertices.write(); 1469 DVector<Vector3>::Write vw = vertices.write();
1470 Vector3 *verticesw=vw.ptr(); 1470 Vector3 *verticesw=vw.ptr();
1471 1471
1472 AABB _aabb; 1472 AABB _aabb;
1473 1473
1474 1474
1475 for(int i=0;i<src_face_count;i++) { 1475 for(int i=0;i<src_face_count;i++) {
1476 1476
1477 Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] ); 1477 Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] );
1478 1478
1479 bvh_arrayw[i].aabb=face.get_aabb(); 1479 bvh_arrayw[i].aabb=face.get_aabb();
1480 bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5; 1480 bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5;
1481 bvh_arrayw[i].face_index=i; 1481 bvh_arrayw[i].face_index=i;
1482 facesw[i].indices[0]=i*3+0; 1482 facesw[i].indices[0]=i*3+0;
1483 facesw[i].indices[1]=i*3+1; 1483 facesw[i].indices[1]=i*3+1;
1484 facesw[i].indices[2]=i*3+2; 1484 facesw[i].indices[2]=i*3+2;
1485 facesw[i].normal=face.get_plane().normal; 1485 facesw[i].normal=face.get_plane().normal;
1486 verticesw[i*3+0]=face.vertex[0]; 1486 verticesw[i*3+0]=face.vertex[0];
1487 verticesw[i*3+1]=face.vertex[1]; 1487 verticesw[i*3+1]=face.vertex[1];
1488 verticesw[i*3+2]=face.vertex[2]; 1488 verticesw[i*3+2]=face.vertex[2];
1489 if (i==0) 1489 if (i==0)
1490 _aabb=bvh_arrayw[i].aabb; 1490 _aabb=bvh_arrayw[i].aabb;
1491 else 1491 else
1492 _aabb.merge_with(bvh_arrayw[i].aabb); 1492 _aabb.merge_with(bvh_arrayw[i].aabb);
1493 1493
1494 } 1494 }
1495 1495
1496 w=DVector<Face>::Write(); 1496 w=DVector<Face>::Write();
1497 vw=DVector<Vector3>::Write(); 1497 vw=DVector<Vector3>::Write();
1498 1498
1499 int count=0; 1499 int count=0;
1500 _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count); 1500 _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count);
1501 1501
1502 bvh.resize( count+1 ); 1502 bvh.resize( count+1 );
1503 1503
1504 DVector<BVH>::Write bvhw2 = bvh.write(); 1504 DVector<BVH>::Write bvhw2 = bvh.write();
1505 BVH*bvh_arrayw2=bvhw2.ptr(); 1505 BVH*bvh_arrayw2=bvhw2.ptr();
1506 1506
1507 int idx=0; 1507 int idx=0;
1508 _fill_bvh(bvh_tree,bvh_arrayw2,idx); 1508 _fill_bvh(bvh_tree,bvh_arrayw2,idx);
1509 1509
1510 configure(_aabb); // this type of shape has no margin 1510 configure(_aabb); // this type of shape has no margin
1511 1511
1512 1512
1513#endif 1513#endif
1514} 1514}
1515 1515
1516 1516
1517void ConcavePolygonShapeSW::set_data(const Variant& p_data) { 1517void ConcavePolygonShapeSW::set_data(const Variant& p_data) {
1518 1518
1519 1519
1520 _setup(p_data); 1520 _setup(p_data);
1521} 1521}
1522 1522
1523Variant ConcavePolygonShapeSW::get_data() const { 1523Variant ConcavePolygonShapeSW::get_data() const {
1524 1524
1525 return get_faces(); 1525 return get_faces();
1526} 1526}
1527 1527
1528ConcavePolygonShapeSW::ConcavePolygonShapeSW() { 1528ConcavePolygonShapeSW::ConcavePolygonShapeSW() {
1529 1529
1530 1530
1531} 1531}
1532 1532
1533 1533
1534 1534
1535/* HEIGHT MAP SHAPE */ 1535/* HEIGHT MAP SHAPE */
1536 1536
1537DVector<float> HeightMapShapeSW::get_heights() const { 1537DVector<float> HeightMapShapeSW::get_heights() const {
1538 1538
1539 return heights; 1539 return heights;
1540} 1540}
1541int HeightMapShapeSW::get_width() const { 1541int HeightMapShapeSW::get_width() const {
1542 1542
1543 return width; 1543 return width;
1544} 1544}
1545int HeightMapShapeSW::get_depth() const { 1545int HeightMapShapeSW::get_depth() const {
1546 1546
1547 return depth; 1547 return depth;
1548} 1548}
1549float HeightMapShapeSW::get_cell_size() const { 1549float HeightMapShapeSW::get_cell_size() const {
1550 1550
1551 return cell_size; 1551 return cell_size;
1552} 1552}
1553 1553
1554 1554
1555void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { 1555void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
1556 1556
1557 //not very useful, but not very used either 1557 //not very useful, but not very used either
1558 p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max ); 1558 p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );
1559 1559
1560} 1560}
1561 1561
1562Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const { 1562Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const {
1563 1563
1564 1564
1565 //not very useful, but not very used either 1565 //not very useful, but not very used either
1566 return get_aabb().get_support(p_normal); 1566 return get_aabb().get_support(p_normal);
1567 1567
1568} 1568}
1569 1569
1570bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const { 1570bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const {
1571 1571
1572 1572
1573 return false; 1573 return false;
1574} 1574}
1575 1575
1576 1576
1577void HeightMapShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const { 1577void HeightMapShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
1578 1578
1579 1579
1580 1580
1581} 1581}
1582 1582
1583 1583
1584Vector3 HeightMapShapeSW::get_moment_of_inertia(float p_mass) const { 1584Vector3 HeightMapShapeSW::get_moment_of_inertia(float p_mass) const {
1585 1585
1586 1586
1587 // use crappy AABB approximation 1587 // use crappy AABB approximation
1588 Vector3 extents=get_aabb().size*0.5; 1588 Vector3 extents=get_aabb().size*0.5;
1589 1589
1590 return Vector3( 1590 return Vector3(
1591 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z), 1591 (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
1592 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z), 1592 (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
1593 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y) 1593 (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
1594 ); 1594 );
1595} 1595}
1596 1596
1597 1597
1598void HeightMapShapeSW::_setup(DVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) { 1598void HeightMapShapeSW::_setup(DVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) {
1599 1599
1600 heights=p_heights; 1600 heights=p_heights;
1601 width=p_width; 1601 width=p_width;
1602 depth=p_depth;; 1602 depth=p_depth;;
1603 cell_size=p_cell_size; 1603 cell_size=p_cell_size;
1604 1604
1605 DVector<real_t>::Read r = heights. read(); 1605 DVector<real_t>::Read r = heights. read();
1606 1606
1607 AABB aabb; 1607 AABB aabb;
1608 1608
1609 for(int i=0;i<depth;i++) { 1609 for(int i=0;i<depth;i++) {
1610 1610
1611 for(int j=0;j<width;j++) { 1611 for(int j=0;j<width;j++) {
1612 1612
1613 float h = r[i*width+j]; 1613 float h = r[i*width+j];
1614 1614
1615 Vector3 pos( j*cell_size, h, i*cell_size ); 1615 Vector3 pos( j*cell_size, h, i*cell_size );
1616 if (i==0 || j==0) 1616 if (i==0 || j==0)
1617 aabb.pos=pos; 1617 aabb.pos=pos;
1618 else 1618 else
1619 aabb.expand_to(pos); 1619 aabb.expand_to(pos);
1620 1620
1621 } 1621 }
1622 } 1622 }
1623 1623
1624 1624
1625 configure(aabb); 1625 configure(aabb);
1626} 1626}
1627 1627
1628void HeightMapShapeSW::set_data(const Variant& p_data) { 1628void HeightMapShapeSW::set_data(const Variant& p_data) {
1629 1629
1630 ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY ); 1630 ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY );
1631 Dictionary d=p_data; 1631 Dictionary d=p_data;
1632 ERR_FAIL_COND( !d.has("width") ); 1632 ERR_FAIL_COND( !d.has("width") );
1633 ERR_FAIL_COND( !d.has("depth") ); 1633 ERR_FAIL_COND( !d.has("depth") );
1634 ERR_FAIL_COND( !d.has("cell_size") ); 1634 ERR_FAIL_COND( !d.has("cell_size") );
1635 ERR_FAIL_COND( !d.has("heights") ); 1635 ERR_FAIL_COND( !d.has("heights") );
1636 1636
1637 int width=d["width"]; 1637 int width=d["width"];
1638 int depth=d["depth"]; 1638 int depth=d["depth"];
1639 float cell_size=d["cell_size"]; 1639 float cell_size=d["cell_size"];
1640 DVector<float> heights=d["heights"]; 1640 DVector<float> heights=d["heights"];
1641 1641
1642 ERR_FAIL_COND( width<= 0); 1642 ERR_FAIL_COND( width<= 0);
1643 ERR_FAIL_COND( depth<= 0); 1643 ERR_FAIL_COND( depth<= 0);
1644 ERR_FAIL_COND( cell_size<= CMP_EPSILON); 1644 ERR_FAIL_COND( cell_size<= CMP_EPSILON);
1645 ERR_FAIL_COND( heights.size() != (width*depth) ); 1645 ERR_FAIL_COND( heights.size() != (width*depth) );
1646 _setup(heights, width, depth, cell_size ); 1646 _setup(heights, width, depth, cell_size );
1647 1647
1648} 1648}
1649 1649
1650Variant HeightMapShapeSW::get_data() const { 1650Variant HeightMapShapeSW::get_data() const {
1651 1651
1652 ERR_FAIL_V(Variant()); 1652 ERR_FAIL_V(Variant());
1653 1653
1654} 1654}
1655 1655
1656HeightMapShapeSW::HeightMapShapeSW() { 1656HeightMapShapeSW::HeightMapShapeSW() {
1657 1657
1658 width=0; 1658 width=0;
1659 depth=0; 1659 depth=0;
1660 cell_size=0; 1660 cell_size=0;
1661} 1661}
1662 1662
1663 1663
1664 1664
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
index 69c82519d..070bc5e06 100644
--- a/servers/physics_server.cpp
+++ b/servers/physics_server.cpp
@@ -85,6 +85,7 @@ void PhysicsDirectBodyState::_bind_methods() {
85 ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsDirectBodyState::get_transform); 85 ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsDirectBodyState::get_transform);
86 86
87 ObjectTypeDB::bind_method(_MD("add_force","force","pos"),&PhysicsDirectBodyState::add_force); 87 ObjectTypeDB::bind_method(_MD("add_force","force","pos"),&PhysicsDirectBodyState::add_force);
88 ObjectTypeDB::bind_method(_MD("apply_impulse","pos","j"),&PhysicsDirectBodyState::apply_impulse);
88 89
89 ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state); 90 ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state);
90 ObjectTypeDB::bind_method(_MD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping); 91 ObjectTypeDB::bind_method(_MD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping);
diff --git a/servers/physics_server.h b/servers/physics_server.h
index da51dbc8e..955caffe5 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -58,6 +58,7 @@ public:
58 virtual Transform get_transform() const=0; 58 virtual Transform get_transform() const=0;
59 59
60 virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0; 60 virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0;
61 virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j)=0;
61 62
62 virtual void set_sleep_state(bool p_enable)=0; 63 virtual void set_sleep_state(bool p_enable)=0;
63 virtual bool is_sleeping() const=0; 64 virtual bool is_sleeping() const=0;
diff --git a/servers/visual/rasterizer.h b/servers/visual/rasterizer.h
index 2b02a81a4..77d4da81d 100644
--- a/servers/visual/rasterizer.h
+++ b/servers/visual/rasterizer.h
@@ -507,6 +507,7 @@ public:
507 float octree_lattice_size; 507 float octree_lattice_size;
508 float octree_lattice_divide; 508 float octree_lattice_divide;
509 float texture_multiplier; 509 float texture_multiplier;
510 float lightmap_multiplier;
510 int octree_steps; 511 int octree_steps;
511 Vector2 octree_tex_pixel_size; 512 Vector2 octree_tex_pixel_size;
512 }; 513 };
@@ -520,6 +521,7 @@ public:
520 Vector<float> morph_values; 521 Vector<float> morph_values;
521 BakedLightData *baked_light; 522 BakedLightData *baked_light;
522 Transform *baked_light_octree_xform; 523 Transform *baked_light_octree_xform;
524 int baked_lightmap_id;
523 bool mirror :8; 525 bool mirror :8;
524 bool depth_scale :8; 526 bool depth_scale :8;
525 bool billboard :8; 527 bool billboard :8;
diff --git a/servers/visual/shader_language.cpp b/servers/visual/shader_language.cpp
index 16a725010..489b5c377 100644
--- a/servers/visual/shader_language.cpp
+++ b/servers/visual/shader_language.cpp
@@ -1005,6 +1005,11 @@ const ShaderLanguage::OperatorDef ShaderLanguage::operator_defs[]={
1005 1005
1006const ShaderLanguage::BuiltinsDef ShaderLanguage::vertex_builtins_defs[]={ 1006const ShaderLanguage::BuiltinsDef ShaderLanguage::vertex_builtins_defs[]={
1007 1007
1008 { "SRC_VERTEX", TYPE_VEC3},
1009 { "SRC_NORMAL", TYPE_VEC3},
1010 { "SRC_TANGENT", TYPE_VEC3},
1011 { "SRC_BINORMALF", TYPE_FLOAT},
1012
1008 { "VERTEX", TYPE_VEC3}, 1013 { "VERTEX", TYPE_VEC3},
1009 { "NORMAL", TYPE_VEC3}, 1014 { "NORMAL", TYPE_VEC3},
1010 { "TANGENT", TYPE_VEC3}, 1015 { "TANGENT", TYPE_VEC3},
@@ -1023,6 +1028,7 @@ const ShaderLanguage::BuiltinsDef ShaderLanguage::vertex_builtins_defs[]={
1023 { "WORLD_MATRIX", TYPE_MAT4}, 1028 { "WORLD_MATRIX", TYPE_MAT4},
1024 { "INV_CAMERA_MATRIX", TYPE_MAT4}, 1029 { "INV_CAMERA_MATRIX", TYPE_MAT4},
1025 { "PROJECTION_MATRIX", TYPE_MAT4}, 1030 { "PROJECTION_MATRIX", TYPE_MAT4},
1031 { "MODELVIEW_MATRIX", TYPE_MAT4},
1026 { "INSTANCE_ID", TYPE_FLOAT}, 1032 { "INSTANCE_ID", TYPE_FLOAT},
1027 { "TIME", TYPE_FLOAT}, 1033 { "TIME", TYPE_FLOAT},
1028 { NULL, TYPE_VOID}, 1034 { NULL, TYPE_VOID},
diff --git a/servers/visual/visual_server_raster.cpp b/servers/visual/visual_server_raster.cpp
index 7c1f03b71..66862ece6 100644
--- a/servers/visual/visual_server_raster.cpp
+++ b/servers/visual/visual_server_raster.cpp
@@ -148,7 +148,7 @@ String VisualServerRaster::shader_get_fragment_code(RID p_shader) const{
148 148
149String VisualServerRaster::shader_get_light_code(RID p_shader) const{ 149String VisualServerRaster::shader_get_light_code(RID p_shader) const{
150 150
151 return rasterizer->shader_get_fragment_code(p_shader); 151 return rasterizer->shader_get_light_code(p_shader);
152} 152}
153 153
154void VisualServerRaster::shader_get_param_list(RID p_shader, List<PropertyInfo> *p_param_list) const { 154void VisualServerRaster::shader_get_param_list(RID p_shader, List<PropertyInfo> *p_param_list) const {
@@ -1038,6 +1038,7 @@ RID VisualServerRaster::baked_light_create() {
1038 baked_light->data.octree_lattice_size=0; 1038 baked_light->data.octree_lattice_size=0;
1039 baked_light->data.octree_lattice_divide=0; 1039 baked_light->data.octree_lattice_divide=0;
1040 baked_light->data.octree_steps=1; 1040 baked_light->data.octree_steps=1;
1041 baked_light->data.lightmap_multiplier=1.0;
1041 1042
1042 return baked_light_owner.make_rid( baked_light ); 1043 return baked_light_owner.make_rid( baked_light );
1043 1044
@@ -1063,6 +1064,26 @@ VisualServer::BakedLightMode VisualServerRaster::baked_light_get_mode(RID p_bake
1063 1064
1064} 1065}
1065 1066
1067void VisualServerRaster::baked_light_set_lightmap_multiplier(RID p_baked_light,float p_multiplier) {
1068
1069 VS_CHANGED;
1070 BakedLight *baked_light = baked_light_owner.get(p_baked_light);
1071 ERR_FAIL_COND(!baked_light);
1072
1073 baked_light->data.lightmap_multiplier=p_multiplier;
1074
1075}
1076
1077float VisualServerRaster::baked_light_get_lightmap_multiplier(RID p_baked_light) const{
1078
1079 const BakedLight *baked_light = baked_light_owner.get(p_baked_light);
1080 ERR_FAIL_COND_V(!baked_light,0);
1081
1082 return baked_light->data.lightmap_multiplier;
1083
1084}
1085
1086
1066void VisualServerRaster::baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree){ 1087void VisualServerRaster::baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree){
1067 1088
1068 VS_CHANGED; 1089 VS_CHANGED;
@@ -2568,7 +2589,7 @@ void VisualServerRaster::instance_geometry_set_baked_light_texture_index(RID p_i
2568 Instance *instance = instance_owner.get( p_instance ); 2589 Instance *instance = instance_owner.get( p_instance );
2569 ERR_FAIL_COND( !instance ); 2590 ERR_FAIL_COND( !instance );
2570 2591
2571 instance->lightmap_texture_index=p_tex_id; 2592 instance->data.baked_lightmap_id=p_tex_id;
2572 2593
2573 2594
2574} 2595}
@@ -2577,7 +2598,7 @@ int VisualServerRaster::instance_geometry_get_baked_light_texture_index(RID p_in
2577 const Instance *instance = instance_owner.get( p_instance ); 2598 const Instance *instance = instance_owner.get( p_instance );
2578 ERR_FAIL_COND_V( !instance,0 ); 2599 ERR_FAIL_COND_V( !instance,0 );
2579 2600
2580 return instance->lightmap_texture_index; 2601 return instance->data.baked_lightmap_id;
2581 2602
2582} 2603}
2583 2604
diff --git a/servers/visual/visual_server_raster.h b/servers/visual/visual_server_raster.h
index 0368780bf..2620225cc 100644
--- a/servers/visual/visual_server_raster.h
+++ b/servers/visual/visual_server_raster.h
@@ -157,7 +157,7 @@ class VisualServerRaster : public VisualServer {
157 float draw_range_begin; 157 float draw_range_begin;
158 float draw_range_end; 158 float draw_range_end;
159 float extra_margin; 159 float extra_margin;
160 int lightmap_texture_index; 160
161 161
162 162
163 Rasterizer::InstanceData data; 163 Rasterizer::InstanceData data;
@@ -267,6 +267,7 @@ class VisualServerRaster : public VisualServer {
267 data.billboard_y=false; 267 data.billboard_y=false;
268 data.baked_light=NULL; 268 data.baked_light=NULL;
269 data.baked_light_octree_xform=NULL; 269 data.baked_light_octree_xform=NULL;
270 data.baked_lightmap_id=-1;
270 version=1; 271 version=1;
271 room_info=NULL; 272 room_info=NULL;
272 room=NULL; 273 room=NULL;
@@ -278,7 +279,7 @@ class VisualServerRaster : public VisualServer {
278 draw_range_end=0; 279 draw_range_end=0;
279 extra_margin=0; 280 extra_margin=0;
280 visible_in_all_rooms=false; 281 visible_in_all_rooms=false;
281 lightmap_texture_index=-1; 282
282 baked_light=NULL; 283 baked_light=NULL;
283 baked_light_info=NULL; 284 baked_light_info=NULL;
284 BLE=NULL; 285 BLE=NULL;
@@ -942,6 +943,9 @@ public:
942 virtual void baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree); 943 virtual void baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree);
943 virtual DVector<uint8_t> baked_light_get_octree(RID p_baked_light) const; 944 virtual DVector<uint8_t> baked_light_get_octree(RID p_baked_light) const;
944 945
946 virtual void baked_light_set_lightmap_multiplier(RID p_baked_light,float p_multiplier);
947 virtual float baked_light_get_lightmap_multiplier(RID p_baked_light) const;
948
945 virtual void baked_light_add_lightmap(RID p_baked_light,const RID p_texture,int p_id); 949 virtual void baked_light_add_lightmap(RID p_baked_light,const RID p_texture,int p_id);
946 virtual void baked_light_clear_lightmaps(RID p_baked_light); 950 virtual void baked_light_clear_lightmaps(RID p_baked_light);
947 951
diff --git a/servers/visual/visual_server_wrap_mt.h b/servers/visual/visual_server_wrap_mt.h
index d577ca0c5..f1ba4c453 100644
--- a/servers/visual/visual_server_wrap_mt.h
+++ b/servers/visual/visual_server_wrap_mt.h
@@ -909,6 +909,8 @@ public:
909 FUNC2(baked_light_set_octree,RID,DVector<uint8_t>); 909 FUNC2(baked_light_set_octree,RID,DVector<uint8_t>);
910 FUNC1RC(DVector<uint8_t>,baked_light_get_octree,RID); 910 FUNC1RC(DVector<uint8_t>,baked_light_get_octree,RID);
911 911
912 FUNC2(baked_light_set_lightmap_multiplier,RID,float);
913 FUNC1RC(float,baked_light_get_lightmap_multiplier,RID);
912 914
913 FUNC3(baked_light_add_lightmap,RID,RID,int); 915 FUNC3(baked_light_add_lightmap,RID,RID,int);
914 FUNC1(baked_light_clear_lightmaps,RID); 916 FUNC1(baked_light_clear_lightmaps,RID);
diff --git a/servers/visual_server.h b/servers/visual_server.h
index 9cad17390..7d7b10bed 100644
--- a/servers/visual_server.h
+++ b/servers/visual_server.h
@@ -585,6 +585,9 @@ public:
585 virtual void baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree)=0; 585 virtual void baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree)=0;
586 virtual DVector<uint8_t> baked_light_get_octree(RID p_baked_light) const=0; 586 virtual DVector<uint8_t> baked_light_get_octree(RID p_baked_light) const=0;
587 587
588 virtual void baked_light_set_lightmap_multiplier(RID p_baked_light,float p_multiplier)=0;
589 virtual float baked_light_get_lightmap_multiplier(RID p_baked_light) const=0;
590
588 virtual void baked_light_add_lightmap(RID p_baked_light,const RID p_texture,int p_id)=0; 591 virtual void baked_light_add_lightmap(RID p_baked_light,const RID p_texture,int p_id)=0;
589 virtual void baked_light_clear_lightmaps(RID p_baked_light)=0; 592 virtual void baked_light_clear_lightmaps(RID p_baked_light)=0;
590 593
generated by cgit v1.2.3-70-g09d2 (git 2.51.2) at 2025-11-10 13:56:19 +0000