?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
!I (1670409):
[CRYENGINE.git] / Code / CryEngine / CryPhysics / livingentity.cpp
blobb792a0961383f74adbebeafb6a2da4a9f2c7d4ed
1 // Copyright 2001-2018 Crytek GmbH / Crytek Group. All rights reserved.
2
3 #include "StdAfx.h"
4
5 #include "bvtree.h"
6 #include "geometry.h"
7 #include "singleboxtree.h"
8 #include "cylindergeom.h"
9 #include "capsulegeom.h"
10 #include "spheregeom.h"
11 #include "raybv.h"
12 #include "raygeom.h"
13 #include "rigidbody.h"
14 #include "physicalplaceholder.h"
15 #include "physicalentity.h"
16 #include "geoman.h"
17 #include "physicalworld.h"
18 #include "livingentity.h"
19 #include "rigidentity.h"
20 #include "waterman.h"
21
22 #define collider_flags (m_parts[0].flagsCollider)
23 #define GROUND_EPS_SIZE max(0.01f*m_size.z, 0.004f)
24
25 inline Vec3_tpl<short> EncodeVec6b(const Vec3 &vec) {
26 return Vec3_tpl<short>(
27 float2int(min_safe(100.0f,max_safe(-100.0f,vec.x))*(32767/100.0f)),
28 float2int(min_safe(100.0f,max_safe(-100.0f,vec.y))*(32767/100.0f)),
29 float2int(min_safe(100.0f,max_safe(-100.0f,vec.z))*(32767/100.0f)));
30 }
31 inline Vec3 DecodeVec6b(const Vec3_tpl<short> &vec) {
32 return Vec3(vec.x,vec.y,vec.z)*(100.0f/32767);
33 }
34
35 struct vec4b {
36 short yaw;
37 char pitch;
38 unsigned char len;
39 };
40 inline vec4b EncodeVec4b(const Vec3 &vec) {
41 vec4b res = { 0,0,0 };
42 if (vec.len2()>sqr(1.0f/512)) {
43 float len = vec.len();
44 res.yaw = float2int(atan2_tpl(vec.y,vec.x)*(32767/g_PI));
45 res.pitch = float2int(asin_tpl(vec.z/len)*(127/g_PI));
46 res.len = float2int(min(20.0f,len)*(255/20.0f));
47 }
48 return res;
49 }
50 inline Vec3 DecodeVec4b(const vec4b &vec) {
51 float yaw=vec.yaw*(g_PI/32767), pitch=vec.pitch*(g_PI/127), len=vec.len*(20.0f/255);
52 float t = cos_tpl(pitch);
53 return Vec3(t*cos_tpl(yaw),t*sin_tpl(yaw),sin_tpl(pitch))*len;
54 }
55
56 CLivingEntity::CLivingEntity(CPhysicalWorld *pWorld, IGeneralMemoryHeap* pHeap)
57 : CPhysicalEntity(pWorld, pHeap)
58 , m_vel(ZERO)
59 , m_velRequested(ZERO)
60 , m_gravity(0, 0, -9.81f)
61 , m_nslope(0.0f, 0.0f, 1.0f)
62 , m_dtRequested(0.0f)
63 , m_kInertia(8.0f)
64 , m_kInertiaAccel(0.0f)
65 , m_kAirControl(0.1f)
66 , m_kAirResistance(0.0f)
67 , m_hCyl(1.1f)
68 , m_hEye(1.7f)
69 , m_hPivot(0.0f)
70 , m_size(0.4f, 0.4f, 0.6f)
71 , m_dh(0.0f)
72 , m_dhSpeed(0.0f)
73 , m_dhAcc(0.0f)
74 , m_stablehTime(1.0f)
75 , m_hLatest(-1e-10f)
76 , m_nodSpeed(60.0f)
77 , m_mass(80)
78 , m_massinv(2.0f / 80)
79 , m_surface_idx(0)
80 , m_lastGroundSurfaceIdx(-1)
81 , m_lastGroundSurfaceIdxAux(-1)
82 , m_lastGroundPrim(-1)
83 , m_timeFlying(0.0f)
84 , m_timeForceInertia(0.0f)
85 , m_slopeSlide(cos_tpl(g_PI * 0.2f))
86 , m_slopeClimb(cos_tpl(g_PI * 0.3f))
87 , m_slopeJump(cos_tpl(g_PI*0.3f))
88 , m_slopeFall(cos_tpl(g_PI*0.39f))
89 , m_maxVelGround(10.0f)
90 , m_groundContactEps(GROUND_EPS_SIZE)
91 , m_timeImpulseRecover(5.0f)
92 , m_pCylinderGeom(nullptr)
93 , m_hHead(1.9f)
94 , m_pHeadGeom(nullptr)
95 , m_timeUseLowCap(-1.0f)
96 , m_timeSinceStanceChange(0.0f)
97 , m_timeSinceImpulseContact(10.0f)
98 , m_timeStepFull(0.0f)
99 , m_timeStepPerformed(0.0f)
100 , m_iSnapshot(0)
101 , m_iTimeLastSend(-1)
102 , m_collTypes(ent_terrain | ent_static | ent_sleeping_rigid | ent_rigid | ent_living)
103 , m_pLivingEntToIgnore(nullptr)
104 , m_bFlying(1)
105 , m_bJumpRequested(0)
106 , m_bSwimming(0)
107 , m_bUseCapsule(0)
108 , m_bIgnoreCommands(0)
109 , m_bStateReading(0)
110 , m_bActive(1)
111 , m_bActiveEnvironment(0)
112 , m_bStuck(0)
113 , m_bReleaseGroundColliderWhenNotActive(1)
114 , m_bHadCollisions(0)
115 , m_bSquashed(0)
116 , m_pLastGroundCollider(nullptr)
117 , m_iLastGroundColliderPart(0)
118 , m_posLastGroundColl(ZERO)
119 , m_velGround(ZERO)
120 , m_deltaPos(ZERO)
121 , m_posLocal(ZERO)
122 , m_deltaV(ZERO)
123 , m_deltaQRot(IDENTITY)
124 , m_timeSmooth(0.16f)
125 , m_timeRotChanged(-1.0f)
126 , m_pContacts(nullptr)
127 , m_nContacts(0)
128 , m_nContactsAlloc(0)
129 , m_pBody(nullptr)
130 , m_lockLiving(0)
131 , m_lockStep(0)
132 , m_forceFly(false)
133 {
134 if (pWorld)
135 m_gravity = pWorld->m_vars.gravity;
136
137 m_iSimClass = 3;
138
139 cylinder cyl;
140 cyl.axis.Set(0,0,1); cyl.center.zero();
141 cyl.hh = m_size.z; cyl.r = m_size.x;
142
143 if (m_pWorld) {
144 if (m_bUseCapsule)
145 ((CCapsuleGeom*)(m_pCylinderGeom = new CCapsuleGeom))->CreateCapsule((capsule*)&cyl);
146 else
147 (m_pCylinderGeom = new CCylinderGeom)->CreateCylinder(&cyl);
148 m_pCylinderGeom->CalcPhysicalProperties(&m_CylinderGeomPhys);
149 m_CylinderGeomPhys.pMatMapping = 0;
150 m_CylinderGeomPhys.nMats = 0;
151 m_CylinderGeomPhys.nRefCount = -1;
152 m_parts = m_pWorld->AllocEntityPart(); m_nPartsAlloc = 1;
153 m_parts[0].pPhysGeom = m_parts[0].pPhysGeomProxy = &m_CylinderGeomPhys;
154 m_parts[0].id = 100;
155 m_parts[0].pos.Set(0,0,m_hCyl-m_hPivot);
156 m_parts[0].q.SetIdentity();
157 m_parts[0].scale = 1.0f;
158 m_parts[0].mass = m_mass;
159 m_parts[0].surface_idx = m_surface_idx;
160 m_parts[0].flags = geom_collides|geom_monitor_contacts|geom_floats;
161 m_parts[0].flagsCollider = geom_colltype_player;
162 m_parts[0].minContactDist = m_size.z*0.1;
163 m_parts[0].pNewCoords = (coord_block_BBox*)&m_parts[0].pos;
164 m_parts[0].idmatBreakable = -1;
165 m_parts[0].pLattice = 0;
166 m_parts[0].pMatMapping = 0;
167 m_parts[0].nMats = 0;
168 m_parts[0].pPlaceholder = 0;
169 m_nParts = 1;
170 }
171
172 //CPhysicalEntity
173 m_flags = pef_pushable_by_players | pef_traceable | lef_push_players | lef_push_objects | pef_never_break;
174 m_collisionClass.type |= collision_class_living;
175 }
176
177 CLivingEntity::~CLivingEntity()
178 {
179 for(int i=0;i<m_nParts;i++) {
180 if (!m_parts[i].pPhysGeom || m_parts[i].pMatMapping!=m_parts[i].pPhysGeom->pMatMapping)
181 delete[] m_parts[i].pMatMapping; m_parts[i].pMatMapping=0;
182 }
183 m_parts[0].pPhysGeom = m_parts[0].pPhysGeomProxy = 0;
184 // if (m_history!=m_history_buf) delete[] m_history;
185 // if (m_actions!=m_actions_buf) delete[] m_actions;
186 if (m_pContacts) delete[] m_pContacts;
187 if (m_pCylinderGeom) delete m_pCylinderGeom;
188 if (m_pBody) delete[] m_pBody;
189 if (m_pHeadGeom) delete m_pHeadGeom;
190 }
191
192
193 void CLivingEntity::ReleaseGroundCollider()
194 {
195 if (m_pLastGroundCollider) {
196 m_pLastGroundCollider->Release();
197 m_pLastGroundCollider = 0;
198 }
199 }
200
201 void CLivingEntity::SetGroundCollider(CPhysicalEntity *pCollider, int bAcceptStatic)
202 {
203 ReleaseGroundCollider();
204 if (pCollider && pCollider->m_iSimClass+bAcceptStatic>0) {
205 pCollider->AddRef();
206 m_pLastGroundCollider = pCollider;
207 }
208 }
209
210 int CLivingEntity::Awake(int bAwake,int iSource)
211 {
212 if (m_pLastGroundCollider && m_pLastGroundCollider->m_iSimClass==7)
213 ReleaseGroundCollider();
214 if (!(m_bActiveEnvironment = bAwake)) {
215 m_vel.zero(); m_velRequested.zero(); m_gravity.zero();
216 }
217 return 1;
218 }
219
220
221 float CLivingEntity::UnprojectionNeeded(const Vec3 &pos,const quaternionf &qrot, float hCollider,float hPivot,
222 const Vec3 &newdim, int bCapsule, Vec3 &dirUnproj, int iCaller) const
223 {
224 int i,j,nents;
225 float t;
226 geom_world_data gwd[3];
227 intersection_params ip;
228 CPhysicalEntity **pentlist;
229 geom_contact *pcontacts;
230
231 cylinder newcyl;
232 CCylinderGeom geomCyl;
233 CCapsuleGeom geomCaps;
234 CCylinderGeom *pgeom = bCapsule ? &geomCaps : &geomCyl;
235 newcyl.r = newdim.x; newcyl.hh = newdim.z;
236 newcyl.center.zero();
237 newcyl.axis.Set(0,0,1);
238 geomCyl.CreateCylinder(&newcyl);
239 geomCaps.CreateCapsule((capsule*)&newcyl);
240 Vec3 BBox[2];
241 BBox[0].Set(-newdim.x,-newdim.x,-hPivot) += pos;
242 BBox[1].Set(newdim.x,newdim.x,newdim.z+hCollider-hPivot+newdim.x*bCapsule) += pos;
243
244 nents = m_pWorld->GetEntitiesAround(BBox[0],BBox[1], pentlist, m_collTypes|ent_no_ondemand_activation, 0,0,iCaller);
245 gwd[0].R = Matrix33(qrot);
246 gwd[0].offset = pos + gwd[0].R*Vec3(0,0,hCollider-hPivot);
247 gwd[0].v = -dirUnproj;
248 ip.time_interval = m_size.z*2;
249 for(i=0;i<nents;i++)
250 if (pentlist[i]!=this && (!m_pForeignData || pentlist[i]->m_pForeignData!=m_pForeignData) && !pentlist[i]->IgnoreCollisionsWith(this,1)) {
251 if (pentlist[i]->GetType()!=PE_LIVING) {
252 ReadLock lock(pentlist[i]->m_lockUpdate);
253 for(j=0;j<pentlist[i]->m_nParts;j++) if (pentlist[i]->m_parts[j].flags & collider_flags) {
254 pentlist[i]->GetPartTransform(j, gwd[1].offset,gwd[1].R,gwd[1].scale, this);
255 if (pgeom->Intersect(pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom, gwd,gwd+1, &ip, pcontacts)) {
256 got_unproj:
257 if (dirUnproj.len2()==0) dirUnproj = pcontacts[0].dir;
258 t = pcontacts[0].t; // lock should be released after reading t
259 return t;
260 }
261 for(int ipart=1;ipart<m_nParts;ipart++) if (m_parts[ipart].flagsCollider & pentlist[i]->m_parts[j].flags) {
262 gwd[2].R = Matrix33(qrot);
263 gwd[2].offset = pos + qrot*m_parts[ipart].pos;
264 gwd[2].scale = m_parts[ipart].scale;
265 gwd[2].v = -dirUnproj;
266 if (m_parts[ipart].pPhysGeomProxy->pGeom->Intersect(pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom, gwd+2,gwd+1, &ip, pcontacts))
267 goto got_unproj;
268 }
269 }
270 } else if (pentlist[i]->m_parts[0].flags&collider_flags && !IgnoreCollision(m_collisionClass, pentlist[i]->m_collisionClass)) {
271 CLivingEntity *pent = (CLivingEntity*)pentlist[i];
272 if (fabs_tpl((pos.z+hCollider-hPivot)-(pent->m_pos.z+pent->m_hCyl-pent->m_hPivot)) < newdim.z+pent->m_size.z &&
273 len2(Vec2(pos)-Vec2(pent->m_pos)) < sqr(newdim.x+pent->m_size.x))
274 return 1E10;
275 }
276 }
277 return 0;
278 }
279
280
281 int CLivingEntity::SetParams(pe_params *_params, int bThreadSafe)
282 {
283 ChangeRequest<pe_params> req(this,m_pWorld,_params,bThreadSafe);
284 if (req.IsQueued()) {
285 if (_params->type==pe_player_dimensions::type_id && (unsigned int)m_iSimClass<7u) {
286 int iter=0,iCaller=get_iCaller();
287 WriteLockCond lockc(m_pWorld->m_lockCaller[iCaller], iCaller==MAX_PHYS_THREADS);
288 float hCyl,hPivot;
289 Vec3 size,pos;
290 quaternionf qrot;
291 { ReadLock lock(m_lockUpdate);
292 hCyl=m_hCyl; size=m_size; hPivot=m_hPivot;
293 pos=m_pos; qrot=m_qrot;
294 }
295 pe_player_dimensions *params = (pe_player_dimensions*)_params;
296 Vec3 sizeCollider = is_unused(params->sizeCollider) ? size:params->sizeCollider;
297 float heightCollider = is_unused(params->heightCollider) ? hCyl:params->heightCollider;
298 float unproj=0,step;
299 if ((heightCollider!=hCyl || sizeCollider.x!=size.x || sizeCollider.z!=size.z) && m_pWorld && m_bActive)
300 { do {
301 unproj += (step = UnprojectionNeeded(pos+params->dirUnproj*unproj,qrot,heightCollider,
302 hPivot,sizeCollider,m_bUseCapsule,params->dirUnproj,iCaller))*1.01f;
303 } while (unproj<params->maxUnproj && ++iter<8 && step>0);
304 if (unproj>params->maxUnproj || iter==8)
305 return 0;
306 }
307 }
308 return 1;
309 }
310
311 int res;
312 Vec3 prevpos=m_pos;
313 quaternionf prevq = m_qrot;
314
315 if (res = CPhysicalEntity::SetParams(_params,1)) {
316 if (_params->type==pe_params_pos::type_id && !(((pe_params_pos*)_params)->bRecalcBounds & 32)) {
317 if ((prevpos-m_pos).len2()>sqr(m_size.z*0.01)) {
318 WriteLock lock(m_lockLiving);
319 ReleaseGroundCollider(); m_bFlying = 1;
320 m_vel.zero(); m_dh=0;
321 }
322 for(int i=0;i<3;i++) if (fabs_tpl(m_qrot.v[i])<1e-10f)
323 m_qrot.v[i] = 0;
324 if (fabs_tpl(m_qrot.w)<1e-10f)
325 m_qrot.w = 0;
326 m_qrot.Normalize();
327 if (m_pBody && !(m_pBody->Ibody_inv*Vec3(1)).len2() && m_pWorld->m_lastTimeInterval) {
328 Quat dq = m_qrot*!prevq;
329 m_pBody->w = dq.v.len2()<sqr(0.3f) ? dq.v*2/m_pWorld->m_lastTimeInterval : Vec3(ZERO);
330 m_timeRotChanged = m_pWorld->m_timePhysics;
331 }
332
333 //if ((m_qrot.v^prevq.v).len2() > m_qrot.v.len2()*prevq.v.len2()*sqr(0.001f)) {
334 if (fabs_tpl(m_qrot.v.x)+fabs_tpl(m_qrot.v.y)+fabs_tpl(prevq.v.x)+fabs_tpl(prevq.v.y)>0) {
335 Vec3 dirUnproj(0,0,1);
336 if (((pe_params_pos*)_params)->bRecalcBounds && m_bActive && UnprojectionNeeded(m_pos,m_qrot,m_hCyl,m_hPivot,m_size,m_bUseCapsule,dirUnproj)>0) {
337 pe_params_pos pp; pp.q = prevq;
338 CPhysicalEntity::SetParams(&pp,1);
339 return 0;
340 }
341 }
342 /*if (m_qrot.s!=prevq.s || m_qrot.v!=prevq.v) {
343 Vec3 angles;
344 m_qrot.get_Euler_angles_xyz(angles);
345 m_qrot = quaternionf(angles.z,Vec3(0,0,1));
346 }*/
347 //m_BBox[0].z = min(m_BBox[0].z,m_pos.z-m_hPivot);
348 }
349 return res;
350 }
351
352 if (_params->type==pe_player_dimensions::type_id) {
353 pe_player_dimensions *params = (pe_player_dimensions*)_params;
354 ENTITY_VALIDATE("CLivingEntity:SetParams(player_dimensions)",params);
355 if (is_unused(params->sizeCollider)) params->sizeCollider = m_size;
356 if (is_unused(params->heightCollider)) params->heightCollider = m_hCyl;
357 if (is_unused(params->heightPivot)) params->heightPivot = m_hPivot;
358 if (is_unused(params->bUseCapsule)) params->bUseCapsule = m_bUseCapsule;
359
360 float unproj=0,step; res=0;
361 if ((params->heightCollider!=m_hCyl || params->heightPivot!=m_hPivot || params->sizeCollider.x!=m_size.x || params->sizeCollider.z!=m_size.z ||
362 params->bUseCapsule!=m_bUseCapsule) && m_pWorld && m_bActive)
363 { do {
364 unproj += (step = UnprojectionNeeded(m_pos+params->dirUnproj*unproj,m_qrot,params->heightCollider,
365 params->heightPivot,params->sizeCollider,params->bUseCapsule,params->dirUnproj))*1.01f;
366 } while (unproj<params->maxUnproj && ++res<8 && step>0);
367 if (unproj>params->maxUnproj || res==8)
368 return 0;
369 }
370
371 { WriteLock lock(m_lockUpdate);
372 m_pos += params->dirUnproj*unproj;
373
374 if (params->heightCollider!=m_hCyl || params->heightPivot!=m_hPivot || params->sizeCollider.x!=m_size.x || params->sizeCollider.z!=m_size.z || params->bUseCapsule!=m_bUseCapsule) {
375 m_hPivot = params->heightPivot;
376 cylinder newdim;
377 newdim.r = params->sizeCollider.x;
378 newdim.hh = params->sizeCollider.z;
379 newdim.center.zero();
380 newdim.axis.Set(0,0,1);
381 if (m_bUseCapsule!=params->bUseCapsule) {
382 if (m_pCylinderGeom) delete m_pCylinderGeom;
383 m_CylinderGeomPhys.pGeom = m_pCylinderGeom = params->bUseCapsule ? (new CCapsuleGeom) : (new CCylinderGeom);
384 }
385 if (m_bUseCapsule = params->bUseCapsule)
386 ((CCapsuleGeom*)m_pCylinderGeom)->CreateCapsule((capsule*)&newdim);
387 else
388 m_pCylinderGeom->CreateCylinder(&newdim);
389 m_hCyl = params->heightCollider;
390 m_parts[0].pos.Set(0,0,m_hCyl-m_hPivot);
391 m_size.Set(params->sizeCollider.x, params->sizeCollider.x, params->sizeCollider.z);
392 sphere sph;
393 sph.center.Set(0,0,-m_size.z);
394 sph.r = m_size.x;
395 }
396 if (!is_unused(params->groundContactEps)) m_groundContactEps = max(params->groundContactEps, GROUND_EPS_SIZE);
397
398 if (!is_unused(params->heightEye)) {
399 /*if (m_hEye!=params->heightEye) {
400 m_dh += params->heightEye-m_hEye;
401 m_dhSpeed = 4*(params->heightEye-m_hEye);
402 }*/
403 m_hEye = params->heightEye;
404 m_timeSinceStanceChange = 0;
405 }
406
407 if (!is_unused(params->heightHead)) m_hHead = params->heightHead;
408 if (!is_unused(params->headRadius)) {
409 sphere sph;
410 sph.center.zero(); sph.r = params->headRadius;
411 if (sph.r>0) {
412 if (!m_pHeadGeom) m_pHeadGeom = new CSphereGeom;
413 m_pHeadGeom->CreateSphere(&sph);
414 } else if (m_pHeadGeom) {
415 delete m_pHeadGeom; m_pHeadGeom = 0;
416 }
417 }
418 ComputeBBox(m_BBox);
419 }
420 m_pWorld->RepositionEntity(this,1|8);
421
422 return 1;
423 }
424
425 if (_params->type==pe_player_dynamics::type_id) {
426 pe_player_dynamics *params = (pe_player_dynamics*)_params;
427 WriteLock lock(m_lockLiving);
428 if (!is_unused(params->kInertia)) m_kInertia = params->kInertia;
429 if (!is_unused(params->kInertiaAccel)) m_kInertiaAccel = params->kInertiaAccel;
430 if (!is_unused(params->kAirControl)) m_kAirControl = params->kAirControl;
431 if (!is_unused(params->kAirResistance)) m_kAirResistance = params->kAirResistance;
432 if (!is_unused(params->gravity)) m_gravity = params->gravity;
433 else if (!is_unused(params->gravity.z)) m_gravity.Set(0,0,-params->gravity.z);
434 if (!is_unused(params->nodSpeed)) m_nodSpeed = params->nodSpeed;
435 if (!is_unused(params->bSwimming)) m_bSwimming = params->bSwimming;
436 if (!is_unused(params->mass)) { m_mass = params->mass; m_massinv = m_mass>0 ? 1.0f/m_mass:0.0f; }
437 if (!is_unused(params->surface_idx))
438 m_surface_idx = m_parts[0].surface_idx = params->surface_idx;
439 if (!is_unused(params->minSlideAngle)) m_slopeSlide = cos_tpl(params->minSlideAngle*(g_PI/180.0f));
440 if (!is_unused(params->maxClimbAngle)) m_slopeClimb = cos_tpl(params->maxClimbAngle*(g_PI/180.0f));
441 if (!is_unused(params->maxJumpAngle)) m_slopeJump = cos_tpl(params->maxJumpAngle*(g_PI/180.0f));
442 if (!is_unused(params->minFallAngle)) m_slopeFall = cos_tpl(params->minFallAngle*(g_PI/180.0f));
443 if (!is_unused(params->maxVelGround)) m_maxVelGround = params->maxVelGround;
444 if (!is_unused(params->timeImpulseRecover)) m_timeImpulseRecover = params->timeImpulseRecover;
445 if (!is_unused(params->collTypes)) m_collTypes = params->collTypes;
446 if (!is_unused(params->pLivingEntToIgnore)) m_pLivingEntToIgnore=(CPhysicalEntity*)params->pLivingEntToIgnore;
447 if (!is_unused(params->bReleaseGroundColliderWhenNotActive)) m_bReleaseGroundColliderWhenNotActive = params->bReleaseGroundColliderWhenNotActive;
448 if (!is_unused(params->bActive)) {
449 if (m_bActive+params->bActive*2==2)
450 m_bFlying = 1;
451 m_bActive = params->bActive;
452 }
453 return 1;
454 }
455
456 return 0;
457 }
458
459
460 int CLivingEntity::GetParams(pe_params *_params) const
461 {
462 int res;
463 if (res = CPhysicalEntity::GetParams(_params))
464 return res;
465
466 if (_params->type==pe_player_dimensions::type_id) {
467 pe_player_dimensions *params = (pe_player_dimensions*)_params;
468 ReadLock lock(m_lockLiving);
469 params->heightPivot = m_hPivot;
470 params->sizeCollider = m_size;
471 params->heightCollider = m_hCyl;
472 params->heightEye = m_hEye;
473 params->headRadius = m_pHeadGeom ? m_pHeadGeom->m_sphere.r : 0.0f;
474 params->heightHead = m_hHead;
475 params->bUseCapsule = m_bUseCapsule;
476 params->groundContactEps = m_groundContactEps;
477 return 1;
478 }
479
480 if (_params->type==pe_player_dynamics::type_id) {
481 pe_player_dynamics *params = (pe_player_dynamics*)_params;
482 ReadLock lock(m_lockLiving);
483 params->kInertia = m_kInertia;
484 params->kInertiaAccel = m_kInertiaAccel;
485 params->kAirControl = m_kAirControl;
486 params->kAirResistance = m_kAirResistance;
487 params->gravity = m_gravity;
488 params->nodSpeed = m_nodSpeed;
489 params->bSwimming = m_bSwimming;
490 params->mass = m_mass;
491 params->surface_idx = m_surface_idx;
492 params->minSlideAngle = acos_tpl(m_slopeSlide)*(180.0f/g_PI);
493 params->maxClimbAngle = acos_tpl(m_slopeClimb)*(180.0f/g_PI);
494 params->maxJumpAngle = acos_tpl(m_slopeJump)*(180.0f/g_PI);
495 params->minFallAngle = acos_tpl(m_slopeFall)*(180.0f/g_PI);
496 params->maxVelGround = m_maxVelGround;
497 params->timeImpulseRecover = m_timeImpulseRecover;
498 params->collTypes = m_collTypes;
499 params->pLivingEntToIgnore = m_pLivingEntToIgnore;
500 params->bActive = m_bActive;
501 params->bReleaseGroundColliderWhenNotActive = m_bReleaseGroundColliderWhenNotActive;
502 return 1;
503 }
504
505 return 0;
506 }
507
508
509 int CLivingEntity::AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id, int bThreadSafe)
510 {
511 m_parts[0].flags &= ~(geom_colltype_ray|geom_colltype_foliage_proxy);
512 id = CPhysicalEntity::AddGeometry(pgeom,params,id);
513 m_parts[m_nParts-1].flags |= geom_monitor_contacts;
514 return id;
515 }
516
517
518 void CLivingEntity::RemoveGeometry(int id, int bThreadSafe)
519 {
520 CPhysicalEntity::RemoveGeometry(id);
521 if (m_nParts==1)
522 m_parts[0].flags |= geom_colltype_ray;
523 }
524
525
526 int CLivingEntity::GetStatus(pe_status *_status) const
527 {
528 if (_status->type==pe_status_pos::type_id) {
529 pe_status_pos *status = (pe_status_pos*)_status;
530 int res,flags = status->flags;
531 status->flags |= status_thread_safe;
532 { WriteLock lock(m_lockUpdate);
533 Vec3 prevPos = m_pos;
534 /*if (m_pWorld->m_vars.bMultithreaded)
535 if (m_pWorld->m_iLastLogPump>m_timeLogged)
536 m_posLogged = m_pos;
537 else
538 m_pos = m_posLogged;*/
539 if (m_bActive)
540 ((CLivingEntity*)this)->m_pos += m_deltaPos*(m_timeSmooth*(1/0.3f))-m_qrot*Vec3(0,0,m_dh);
541 res = CPhysicalEntity::GetStatus(_status);
542 ((CLivingEntity*)this)->m_pos = prevPos;
543 }
544 status->flags = flags;
545 return res;
546 }
547
548 if (CPhysicalEntity::GetStatus(_status))
549 return 1;
550
551 if (_status->type==pe_status_living::type_id) {
552 pe_status_living *status = (pe_status_living*)_status;
553 ReadLock lock(m_lockLiving);
554 status->bFlying = m_bFlying & m_bActive;
555 status->timeFlying = m_timeFlying;
556 status->camOffset = m_qrot*Vec3(0,0,/*-m_hPivot+m_hEye*/-m_dh);
557 status->vel = m_vel;
558 if (m_pLastGroundCollider)
559 status->vel += m_velGround;
560
561 status->velUnconstrained = m_vel;
562 status->velRequested = m_velRequested;
563 status->velGround = m_velGround;
564 status->groundHeight = m_hLatest;
565 status->groundSlope = m_nslope;
566 status->groundSurfaceIdx = m_lastGroundSurfaceIdx;
567 status->groundSurfaceIdxAux = m_lastGroundSurfaceIdxAux;
568 status->pGroundCollider = m_pLastGroundCollider;
569 status->iGroundColliderPart = m_iLastGroundColliderPart;
570 status->timeSinceStanceChange = m_timeSinceStanceChange;
571 status->bStuck = m_bStuck;
572 status->pLockStep = &m_lockStep;
573 status->bSquashed = m_bSquashed;
574 return 1;
575 } else
576
577 if (_status->type==pe_status_dynamics::type_id) {
578 pe_status_dynamics *status = (pe_status_dynamics*)_status;
579 ReadLock lock(m_lockLiving);
580 status->v = m_vel;
581 if (m_pLastGroundCollider)
582 status->v += m_velGround;
583 float rdt = 1.0f/m_timeSmooth;
584 quaternionf dq = m_deltaQRot;
585 status->a = m_deltaV*rdt;
586 if (dq.v.len2()<sqr(0.05f))
587 status->w = dq.v*(2*rdt);
588 else
589 status->w = dq.v.normalized()*(acos_tpl(dq.w)*2*rdt);
590 status->centerOfMass = (m_BBox[0]+m_BBox[1])*0.5f;
591 status->mass = m_mass;
592 /*for(idx=idx-1&m_szHistory-1; dt<status->time_interval; idx=idx-1&m_szHistory-1) {
593 rdt = 1.0f/m_history[idx].dt; dt += m_history[idx].dt;
594 status->a += (m_history[idx].v-m_history[idx-1&m_szHistory-1].v)*rdt;
595 status->w += (m_history[idx].q.v-m_history[idx-1&m_szHistory-1].q.v)*2*rdt;
596 status->v += m_history[idx].v;
597 }
598 if (dt>0) {
599 rdt = 1.0f/dt;
600 status->v *= dt;
601 status->a *= dt;
602 status->w *= dt;
603 }*/
604 return 1;
605 }
606
607 if (_status->type==pe_status_timeslices::type_id) {
608 pe_status_timeslices *status = (pe_status_timeslices*)_status;
609 ReadLock lock(m_lockLiving);
610 if (!status->pTimeSlices) return 0;
611 int nSlices=0;
612 return nSlices;
613 }
614
615 if (_status->type==pe_status_collisions::type_id) {
616 // pe_status_collisions *status = (pe_status_collisions*)_status;
617 return m_bHadCollisions--;
618 }
619
620 if (_status->type==pe_status_check_stance::type_id) {
621 pe_status_check_stance *status = (pe_status_check_stance*)_status;
622 Vec3 pos=m_pos, size=m_size;
623 quaternionf qrot=m_qrot;
624 float hCollider=m_hCyl;
625 int bCapsule=m_bUseCapsule;
626 if (!is_unused(status->pos)) pos = status->pos;
627 if (!is_unused(status->q)) qrot = status->q;
628 if (!is_unused(status->sizeCollider)) size = status->sizeCollider;
629 if (!is_unused(status->heightCollider)) hCollider = status->heightCollider;
630 if (!is_unused(status->bUseCapsule)) bCapsule = status->bUseCapsule;
631
632 return ((status->unproj = UnprojectionNeeded(pos,qrot,hCollider,m_hPivot,size,bCapsule,status->dirUnproj))==0);
633 }
634
635 return 0;
636 }
637
638
639 int CLivingEntity::Action(pe_action* _action, int bThreadSafe)
640 {
641 ChangeRequest<pe_action> req(this,m_pWorld,_action,bThreadSafe);
642 if (req.IsQueued())
643 return 1;
644
645 if (_action->type==pe_action_move::type_id) {
646 if (!m_bIgnoreCommands) {
647 pe_action_move *action = (pe_action_move*)_action;
648 bool bForceHistory = false;
649 ENTITY_VALIDATE("CLivingEntity:Action(action_move)",action);
650
651 WriteLockCond lock(m_lockLiving,bThreadSafe^1);
652 if (!is_unused(action->dir)) {
653 //m_velRequested.zero();
654
655 /*if (m_kAirControl==1) {
656 m_velRequested = m_vel = action->dir;
657 m_bJumpRequested = action->iJump;
658 } else*/ if (!action->iJump || action->iJump==3) {
659 m_velRequested = action->dir;
660 if (action->iJump==3)
661 m_forceFly = true;
662 //if (m_bFlying && m_vel.len2()>0 && !m_bSwimming && !m_pWorld->m_vars.bFlyMode)
663 // m_velRequested -= m_vel*((action->dir*m_vel)/m_vel.len2());
664 } else {
665 if (action->iJump==2) {
666 m_vel += action->dir; bForceHistory = true; m_forceFly = false;
667 if (action->dir.z>1.0f) {
668 m_timeFlying=0;
669 m_bJumpRequested = 1;
670 }
671 } else {
672 m_vel = action->dir; bForceHistory = true; m_forceFly = false;
673 m_timeFlying=0;
674 m_bJumpRequested = 1;
675 }
676 }
677 if (m_flags & lef_snap_velocities) {
678 m_velRequested = DecodeVec4b(EncodeVec4b(m_velRequested));
679 m_vel = DecodeVec6b(EncodeVec6b(m_vel));
680 }
681 m_dtRequested = action->dt;
682 }
683 return 1;
684 }
685 return 0;
686 }
687
688 if (_action->type==pe_action_impulse::type_id) {
689 pe_action_impulse *action = (pe_action_impulse*)_action;
690 ENTITY_VALIDATE("CLivingEntity:Action(action_impulse)",action);
691
692 WriteLock lock(m_lockLiving);
693 Vec3 impulse = action->impulse;
694 if (action->iSource==2) // explosion
695 impulse *= 0.3f;
696 if (action->iApplyTime==0) {
697 //if (!m_bFlying) m_velRequested.zero();
698 m_vel += impulse*m_massinv;
699 //m_velRequested = m_vel;
700 if (m_flags & lef_snap_velocities)
701 m_vel = DecodeVec6b(EncodeVec6b(m_vel));
702 if (impulse.z*m_massinv>1.0f) {
703 m_bFlying=1; m_timeFlying=0;
704 }
705 } else {
706 pe_action_move am;
707 am.dir = impulse*m_massinv;
708 am.iJump = 2;
709 Action(&am,1);
710 }
711 /*if (!m_bFlying && action->impulse.z<0 && m_dh<m_size.z*0.1f) {
712 m_dhSpeed = action->impulse.z*m_massinv;
713 m_dhAcc = max(sqr(m_dhSpeed)/(m_size.z*2), m_nodSpeed*0.15f);
714 }*/
715 if (m_kInertia==0)
716 m_timeForceInertia = m_timeImpulseRecover;
717 return 1;
718 }
719
720 if (_action->type==pe_action_reset::type_id) {
721 WriteLock lock(m_lockLiving);
722 m_vel.zero(); m_velRequested.zero();
723 m_dh = m_dhSpeed = 0; m_stablehTime = 1;
724 m_bFlying = 1;
725 return 1;
726 }
727
728 if (_action->type==pe_action_set_velocity::type_id) {
729 pe_action_set_velocity *action = (pe_action_set_velocity*)_action;
730 WriteLock lock(m_lockLiving);
731
732 if (!is_unused(action->v)) {
733 m_velRequested = m_vel = action->v;
734 m_bJumpRequested = 1;
735 }
736
737 return 1;
738 }
739
740 return CPhysicalEntity::Action(_action,1);
741 }
742
743
744 int CLivingEntity::GetStateSnapshot(CStream &stm,float time_back,int flags)
745 {
746 WriteLock lock0(m_lockUpdate),lock1(m_lockLiving);
747 Vec3 pos_prev=m_pos, vel_prev=m_vel, nslope_prev=m_nslope;
748 int bFlying_prev=m_bFlying;
749 float timeFlying_prev=m_timeFlying, timeUseLowCap_prev=m_timeUseLowCap;
750 quaternionf qrot_prev=m_qrot;
751 CPhysicalEntity *pLastGroundCollider=m_pLastGroundCollider;
752 int iLastGroundColliderPart=m_iLastGroundColliderPart;
753 Vec3 posLastGroundColl=m_posLastGroundColl;
754 ReleaseGroundCollider();
755 stm.WriteNumberInBits(SNAPSHOT_VERSION,4);
756 if (m_pWorld->m_vars.bMultiplayer) {
757 WriteCompressedPos(stm,m_pos,(m_id+m_iSnapshot&31)!=0);
758 if (m_pWorld->m_iTimePhysics!=m_iTimeLastSend) {
759 m_iSnapshot++; m_iTimeLastSend = m_pWorld->m_iTimePhysics;
760 }
761 } else
762 stm.Write(m_pos);
763
764 if (m_flags & lef_snap_velocities) {
765 if (m_vel.len2()>0) {
766 stm.Write(true);
767 Vec3_tpl<short> v = EncodeVec6b(m_vel);
768 stm.Write(v.x); stm.Write(v.y); stm.Write(v.z);
769 } else stm.Write(false);
770 if (m_velRequested.len2()>0) {
771 stm.Write(true);
772 vec4b vr = EncodeVec4b(m_velRequested);
773 stm.Write(vr.yaw); stm.Write(vr.pitch); stm.Write(vr.len);
774 } else stm.Write(false);
775 } else {
776 if (m_vel.len2()>0) {
777 stm.Write(true);
778 stm.Write(m_vel);
779 } else stm.Write(false);
780 if (m_velRequested.len2()>0) {
781 stm.Write(true);
782 stm.Write(m_velRequested);
783 } else stm.Write(false);
784 }
785 if (m_timeFlying>0) {
786 stm.Write(true);
787 stm.Write((unsigned short)float2int(m_timeFlying*6553.6f));
788 } else stm.Write(false);
789 unsigned int imft = 0;
790 stm.WriteNumberInBits(imft,2);
791 stm.Write((bool)(m_bFlying!=0));
792
793 /*if (m_pLastGroundCollider) {
794 stm.Write(true);
795 WritePacked(stm, m_pWorld->GetPhysicalEntityId(m_pLastGroundCollider)+1);
796 WritePacked(stm, m_iLastGroundColliderPart);
797 stm.Write((Vec3&)m_posLastGroundColl);
798 } else
799 stm.Write(false);*/
800
801 m_pos=pos_prev; m_vel=vel_prev; m_bFlying=bFlying_prev; m_qrot=qrot_prev;
802 m_timeFlying=timeFlying_prev; m_timeUseLowCap = timeUseLowCap_prev;
803 m_nslope=nslope_prev;
804 SetGroundCollider(pLastGroundCollider);
805 m_iLastGroundColliderPart=iLastGroundColliderPart;
806 m_posLastGroundColl=posLastGroundColl;
807
808 return 1;
809 }
810
811 void SLivingEntityNetSerialize::Serialize( TSerialize ser )
812 {
813 ser.Value( "pos", pos, 'wrld');
814 ser.Value( "vel", vel, 'pLVl');
815 ser.Value( "velRequested", velRequested, 'pLVl');
816 ser.Value( "bFlying", bFlying);
817 ser.Value( "bJumpRequested", bJumpRequested);
818 ser.Value( "dh", dh);
819 ser.Value( "dhSpeed", dhSpeed);
820 ser.Value( "stablehTime", stablehTime);
821 if (ser.BeginOptionalGroup("groundColl", idEntGroundCollider>0)) {
822 ser.Value("idEnt", idEntGroundCollider);
823 ser.Value("ipart", ipartGroundCollider);
824 ser.Value("posGround", posOnGroundCollider);
825 ser.EndGroup();
826 } else {
827 idEntGroundCollider = -1;
828 posOnGroundCollider.zero();
829 }
830 }
831
832 int CLivingEntity::GetStateSnapshot(TSerialize ser, float time_back, int flags)
833 {
834 WriteLock lock0(m_lockUpdate),lock1(m_lockLiving);
835
836 SLivingEntityNetSerialize helper = {
837 m_pos,
838 m_vel,
839 m_velRequested,
840 m_bFlying,
841 m_bJumpRequested,
842 m_dh, m_dhSpeed, m_stablehTime,
843 m_pWorld->GetPhysicalEntityId(m_pLastGroundCollider), m_iLastGroundColliderPart, m_posLastGroundColl
844 };
845 helper.Serialize( ser );
846
847 return 1;
848 }
849
850
851 int CLivingEntity::SetStateFromSnapshot(CStream &stm, int flags)
852 {
853 bool bnz,bCompressed;
854 unsigned short tmp;
855 int ver=0;
856 stm.ReadNumberInBits(ver,4);
857 if (ver!=SNAPSHOT_VERSION)
858 return 0;
859
860 WriteLock lock0(m_lockUpdate),lock1(m_lockLiving);
861 if (flags & ssf_no_update) {
862 Vec3 dummy;
863 if (m_pWorld->m_vars.bMultiplayer)
864 ReadCompressedPos(stm,dummy,bCompressed);
865 else
866 stm.Seek(stm.GetReadPos()+sizeof(Vec3)*8);
867 stm.Read(bnz); if (bnz) stm.Seek(stm.GetReadPos()+((m_flags & lef_snap_velocities) ? 6:sizeof(Vec3))*8);
868 stm.Read(bnz); if (bnz) stm.Seek(stm.GetReadPos()+((m_flags & lef_snap_velocities) ? 4:sizeof(Vec3))*8);
869 stm.Read(bnz); if (bnz) stm.Seek(stm.GetReadPos()+sizeof(tmp)*8);
870 stm.Seek(stm.GetReadPos()+3);
871 return 1;
872 }
873
874 m_posLocal = m_pos + m_deltaPos*(m_timeSmooth*(1/0.3f));
875
876 Vec3 pos0=m_pos/*,vel0=m_vel,velRequested0=m_velRequested*/;
877 // int bFlying0=m_bFlying;
878
879 if (m_pWorld->m_vars.bMultiplayer) {
880 ReadCompressedPos(stm,m_pos,bCompressed);
881 // if we received compressed pos, and our pos0 compressed is equal to it, assume the server had pos equal to our uncompressed pos0
882 if (bCompressed && (CompressPos(pos0)-m_pos).len2()<sqr(0.0001f))
883 m_pos = pos0;
884 } else
885 stm.Read(m_pos);
886
887 if (m_flags & lef_snap_velocities) {
888 stm.Read(bnz); if (bnz) {
889 Vec3_tpl<short> v; stm.Read(v.x); stm.Read(v.y); stm.Read(v.z);
890 m_vel = DecodeVec6b(v);
891 } else m_vel.zero();
892 stm.Read(bnz); if (bnz) {
893 vec4b vr; stm.Read(vr.yaw); stm.Read(vr.pitch); stm.Read(vr.len);
894 m_velRequested = DecodeVec4b(vr);
895 } else m_velRequested.zero();
896 } else {
897 stm.Read(bnz); if (bnz)
898 stm.Read(m_vel);
899 else m_vel.zero();
900 stm.Read(bnz); if (bnz)
901 stm.Read(m_velRequested);
902 else m_velRequested.zero();
903 }
904 stm.Read(bnz); if (bnz) {
905 stm.Read(tmp); m_timeFlying = tmp*(10.0f/65536);
906 } else m_timeFlying = 0;
907 unsigned int imft; stm.ReadNumberInBits(imft,2);
908 static float flytable[] = {0.0f, 0.2f, 0.5f, 1.0f};
909 stm.Read(bnz);
910 m_bFlying = bnz ? 1:0;
911 ReleaseGroundCollider();
912 m_bJumpRequested = 0;
913
914 /*stm.Read(bnz);
915 if (bnz) {
916 SetGroundCollider((CPhysicalEntity*)m_pWorld->GetPhysicalEntityById(ReadPackedInt(stm)-1));
917 m_iLastGroundColliderPart = ReadPackedInt(stm);
918 stm.Read((Vec3&)m_posLastGroundColl);
919 }*/
920
921 #ifdef _DEBUG
922 float diff = (m_pos-pos0).len2();
923 if (diff>sqr(0.0001f) && flags&ssf_compensate_time_diff && m_bActive)
924 m_pWorld->m_pLog->Log("Local client desync: %.4f",sqrt_tpl(diff));
925 #endif
926 if (m_pos.len2()>1E18) {
927 m_pos = pos0;
928 return 1;
929 }
930
931 return 1;
932 }
933
934
935 int CLivingEntity::SetStateFromSnapshot(TSerialize ser, int flags)
936 {
937 SLivingEntityNetSerialize helper;
938 helper.Serialize( ser );
939
940 if ((ser.GetSerializationTarget()==eST_Network) && (helper.pos.len2()<50.0f))
941 return 1;
942
943 if (ser.ShouldCommitValues() && m_pWorld)
944 {
945 pe_params_pos setpos; setpos.bRecalcBounds |= 16|32;
946 pe_action_set_velocity velocity;
947
948 // Vec3 debugOnlyOriginalHelperPos = helper.pos;
949
950 if (flags & ssf_compensate_time_diff)
951 {
952 float dtBack=(m_pWorld->m_iTimeSnapshot[1]-m_pWorld->m_iTimeSnapshot[0])*m_pWorld->m_vars.timeGranularity;
953 helper.pos += helper.vel * dtBack;
954 }
955
956 const float MAX_DIFFERENCE = std::max( helper.vel.GetLength() * 0.1f, 0.1f );
957
958 /*
959 {
960 IPersistantDebug * pPD = gEnv->pGameFramework->GetIPersistantDebug();
961 Vec3 pts[3] = {debugOnlyOriginalHelperPos, helper.pos, m_pos};
962 pPD->Begin("Snap", false);
963 Vec3 bump(0,0,0.2f);
964 for (int i=0; i<3; i++)
965 {
966 Vec3 a = pts[i] + bump;
967 Vec3 b = pts[(i+1)%3] + bump;
968 pPD->AddLine( a, b, ColorF(1,0,0,1), 0.3f );
969 }
970 pPD->Begin("Snap2", true);
971 pPD->AddPlanarDisc( m_pos + bump*0.5f, 0, MAX_DIFFERENCE, ColorF(0,0,1,0.5), 0.3f );
972 }
973 */
974
975 float distance = m_pos.GetDistance(helper.pos);
976 setpos.pos = helper.pos;
977
978 SetParams( &setpos, (m_flags & pef_update)!=0 || get_iCaller()<MAX_PHYS_THREADS); // apply changes immediately for custom-step entities or if called from a phys thread
979
980 WriteLock lock1(m_lockLiving);
981 m_vel = helper.vel;
982 m_velRequested = helper.velRequested;
983 m_bFlying = helper.bFlying;
984 m_bJumpRequested = helper.bJumpRequested;
985 m_dh = helper.dh;
986 m_dhSpeed = helper.dhSpeed;
987 m_stablehTime = helper.stablehTime;
988
989 SetGroundCollider((CPhysicalEntity*)m_pWorld->GetPhysicalEntityById(helper.idEntGroundCollider));
990 m_iLastGroundColliderPart = helper.ipartGroundCollider;
991 m_posLastGroundColl = helper.posOnGroundCollider;
992 }
993
994 return 1;
995 }
996
997
998 float CLivingEntity::ShootRayDown(le_precomp_entity* pents, int nents, le_precomp_part *pparts, const Vec3 &pos,
999 Vec3 &nslope, float time_interval, bool bUseRotation,bool bUpdateGroundCollider,bool bIgnoreSmallObjects)
1000 {
1001 int i,j,jbest,ncont,idbest,idbestAux=-1,iPrim=-1,bHasMatSubst=0;
1002 int j1,iCaller=get_iCaller_int();
1003 Matrix33 R;
1004 Vec3 pt,axis=m_qrot*Vec3(0,0,1);
1005 float h=-1E10f,maxdim,maxarea,haux=-1E10f;
1006 box bbox;
1007 if (bUseRotation)
1008 R = Matrix33(m_qrot);
1009 else {
1010 maxdim = sqr(m_qrot.v.z)+sqr(m_qrot.w);
1011 if (maxdim>0.0001f) {
1012 maxdim = 1/sqrt_tpl(maxdim);
1013 R = Matrix33(m_qrot*Quat(m_qrot.w*maxdim,0,0,m_qrot.v.z*maxdim));
1014 } else
1015 R = Matrix33(Quat(0,1,0,0));
1016 }
1017 const Vec3 dirp = (R*Vec3(0,0,-1));
1018 CRayGeom aray; aray.CreateRay(pos+R*Vec3(0,0,m_hCyl-m_size.z-m_hPivot),R*Vec3(0,0,-1.5f*(m_hCyl-m_size.z)),&dirp);
1019 geom_world_data gwd;
1020 geom_contact *pcontacts;
1021 intersection_params ip;
1022 CPhysicalEntity *pPrevCollider=m_pLastGroundCollider, *pentbest=NULL;
1023 nslope = m_nslope;
1024
1025 for(i=nents-1;i>=0;--i)
1026 if (pents[i].pent!=this && !pents[i].ignoreCollisionsWith) {
1027 PrefetchLine(&pents[i-1],0);
1028 CPhysicalEntity *const pent = pents[i].pent;
1029 for(j=0; j<pent->m_nColliders; j++)
1030 if (pent->m_pColliders[j]==this && pent->HasConstraintContactsWith(pent->m_pColliders[j]))
1031 break;
1032 if (j<pent->m_nColliders)
1033 continue;
1034 for(j1=pents[i].iPartsBegin;j1<pents[i].iPartsEnd;bHasMatSubst|=pparts[j1++].partflags & geom_mat_substitutor)
1035 if ((pparts[j1].partflags & collider_flags) && !(pparts[j1].partflags & geom_no_coll_response)) {
1036 PrefetchLine(&pparts[j1+1],0);
1037 if (bIgnoreSmallObjects && (pentbest || pent->GetRigidBody(j)->v.len2()>1)) {
1038 pparts[j1].pgeom->GetBBox(&bbox);
1039 maxarea = max(max(bbox.size.x*bbox.size.y, bbox.size.x*bbox.size.z), bbox.size.y*bbox.size.z)*sqr(pparts[j1].partscale)*4;
1040 maxdim = max(max(bbox.size.x,bbox.size.y),bbox.size.z)*pparts[j1].partscale*2;
1041 if (maxarea<sqr(m_size.x)*g_PI*0.25f && maxdim<m_size.z*1.4f)
1042 continue;
1043 }
1044 gwd.R = Matrix33(pparts[j1].partrot);
1045 gwd.offset = pparts[j1].partoff;
1046 gwd.scale = pparts[j1].partscale;
1047 if (ncont=pparts[j1].pgeom->Intersect(&aray, &gwd,0, &ip, pcontacts)) {
1048 if (pcontacts[ncont-1].n*aray.m_ray.dir<0) {
1049 if (pparts[j1].partflags & geom_colltype0 && pcontacts[ncont-1].pt*axis>haux) {
1050 haux = (pt=pcontacts[ncont-1].pt)*axis;
1051 idbestAux = pent->GetMatId(pcontacts[ncont-1].id[0], pparts[j1].ipart);
1052 }
1053 if (pcontacts[ncont-1].pt*axis>h) {
1054 h = (pt=pcontacts[ncont-1].pt)*axis; pentbest=pent; jbest=pparts[j1].ipart; nslope = pcontacts[ncont-1].n;
1055 if (pents[i].iSimClass==3) {
1056 nslope = pcontacts[ncont-1].pt-pent->m_pos;
1057 nslope -= axis*(axis*nslope);
1058 if (nslope.len2()>1E-4f)
1059 nslope.normalize();
1060 else nslope = m_qrot*Vec3(1,0,0);
1061 nslope = nslope*0.965925826f + axis*0.2588190451f; // make it act like a 75 degrees slope
1062 }
1063 idbest = pent->GetMatId(pcontacts[ncont-1].id[0], pparts[j1].ipart);
1064 iPrim = pcontacts[ncont-1].iPrim[0];
1065 }
1066 }
1067 }
1068 }
1069 }
1070
1071 if (bUpdateGroundCollider) {
1072 m_lastGroundSurfaceIdx=m_lastGroundSurfaceIdxAux = -1;
1073 if (pentbest) {
1074 SetGroundCollider(pentbest, (pentbest->m_parts[jbest].flags & geom_manually_breakable)!=0);
1075 m_iLastGroundColliderPart = jbest;
1076 Vec3 coll_origin; quaternionf coll_q; float coll_scale;
1077 pentbest->GetPartTransform(jbest, coll_origin,coll_q,coll_scale, this);
1078 m_posLastGroundColl = ((pt-coll_origin)*coll_q)/coll_scale;
1079 int mask = (2<<pentbest->m_iSimClass)-(pentbest->m_id>>31);
1080 if (bHasMatSubst) for(i=nents-1;i>=0;--i) for(j1=pents[i].iPartsBegin;j1<pents[i].iPartsEnd;++j1)
1081 if (pparts[j1].partflags & geom_mat_substitutor && pents[i].pent->m_parts[pparts[j1].ipart].flagsCollider & mask &&
1082 pparts[j1].pgeom->PointInsideStatus(((pt-pparts[j1].partoff)*pparts[j1].partrot)*(fabs_tpl(pparts[j1].partscale-1)>1e-4f ? 1.0f/pparts[j1].partscale:1.0f)))
1083 idbest=idbestAux = pents[i].pent->GetMatId(pparts[j1].surface_idx, pparts[j1].ipart);
1084 m_lastGroundSurfaceIdx = idbest;
1085 m_lastGroundSurfaceIdxAux = idbestAux;
1086 m_lastGroundPrim = iPrim;
1087
1088 if (pPrevCollider!=pentbest)
1089 AddLegsImpulse(m_gravity*time_interval+m_vel,nslope,true);
1090 else
1091 AddLegsImpulse(m_gravity*time_interval,nslope,false);
1092
1093 pe_status_dynamics sd;
1094 sd.ipart = jbest;
1095 pentbest->GetStatus(&sd);
1096 m_velGround = sd.v+(sd.w^pt-sd.centerOfMass);
1097 }
1098 else if (m_pLastGroundCollider)
1099 ReleaseGroundCollider();
1100 }
1101 return h;
1102 }
1103
1104
1105 void CLivingEntity::AddLegsImpulse(const Vec3 &vel, const Vec3 &nslope, bool bInstantChange)
1106 {
1107 int ncoll;
1108 CPhysicalEntity **pColliders/*,*pPrevCollider=m_pLastGroundCollider*/;
1109 pe_status_sample_contact_area ssca;
1110 RigidBody body(false),*pbody;
1111 pe_action_impulse ai;
1112 ssca.ptTest = m_pos-m_qrot*Vec3(0,0,m_hPivot); ssca.dirTest = m_gravity;
1113
1114 if (m_pLastGroundCollider && m_flags&lef_push_objects &&
1115 (unsigned int)m_pLastGroundCollider->m_iSimClass-1u<2u && m_pLastGroundCollider->m_flags & pef_pushable_by_players &&
1116 ((pbody=m_pLastGroundCollider->GetRigidBodyTrans(&body,m_iLastGroundColliderPart,this))->Minv<m_massinv*10 ||
1117 (ncoll=m_pLastGroundCollider->GetColliders(pColliders))==0 || (ncoll==1 && pColliders[0]==m_pLastGroundCollider)) &&
1118 !m_pLastGroundCollider->GetStatus(&ssca))
1119 {
1120 Vec3 vrel = vel;
1121 ai.point = ssca.ptTest;
1122 if (bInstantChange)
1123 vrel -= pbody->v+(pbody->w^ai.point-pbody->pos);
1124 Matrix33 K; K.SetZero();
1125 RigidBody body(false), *pbody=m_pLastGroundCollider->GetRigidBodyTrans(&body, m_iLastGroundColliderPart, this, 0,true);
1126 pbody->GetContactMatrix(ai.point-pbody->pos, K);
1127 K(0,0)+=m_massinv; K(1,1)+=m_massinv; K(2,2)+=m_massinv;
1128 ai.impulse = nslope*(min(0.0f,nslope*vrel)/(nslope*K*nslope));
1129 m_pLastGroundCollider->Action(&ai,1);
1130 }
1131 }
1132
1133
1134 void CLivingEntity::RegisterContact(const Vec3 &posSelf, const Vec3& pt,const Vec3& n, CPhysicalEntity *pCollider, int ipart,int idmat,
1135 float imp, int bLegsContact, int iPrim, int ipartMin)
1136 {
1137 if (m_flags & (pef_monitor_collisions | pef_log_collisions) && !m_bStateReading) {
1138 EventPhysCollision epc;
1139 epc.pEntity[0] = this;
1140 epc.pForeignData[0] = m_pForeignData;
1141 epc.iForeignData[0] = m_iForeignData;
1142 epc.pEntity[1] = pCollider;
1143 epc.pForeignData[1] = pCollider->m_pForeignData;
1144 epc.iForeignData[1] = pCollider->m_iForeignData;
1145 RigidBody body(false), *pbody = pCollider->GetRigidBodyTrans(&body,ipart,this);
1146 Vec3 v = pbody->v+(pbody->w^pt-pbody->pos);
1147 epc.n = n;
1148 if (bLegsContact) {
1149 if (!(pCollider->m_parts[ipart].flags & geom_manually_breakable)) {
1150 if (!((v-m_vel-m_velGround).len2()>3 && pbody->V<m_size.GetVolume()*8))
1151 return;
1152 epc.n = v.normalized();
1153 imp = (epc.n*(v-m_vel-m_velGround))*(m_mass*(m_mass*pbody->Minv+1.0f));
1154 }
1155 }
1156 epc.pt = pt;
1157 epc.vloc[0] = m_vel+m_velGround;
1158 epc.vloc[1] = v;
1159 epc.mass[0] = m_mass;
1160 epc.mass[1] = pbody->M;
1161 epc.idCollider = m_pWorld->GetPhysicalEntityId(pCollider);
1162 epc.partid[0] = m_parts[ipartMin].id;
1163 epc.partid[1] = pCollider->m_parts[ipart].id;
1164 epc.idmat[0] = m_surface_idx;
1165 epc.idmat[1] = pCollider->GetMatId(idmat,ipart);
1166 epc.penetration = epc.radius = 0;
1167 epc.normImpulse = max(0.0f,imp);
1168 if (pCollider->m_parts[ipart].flags & geom_manually_breakable) {
1169 //if (!(pCollider->m_parts[ipart].flags & geom_can_modify))
1170 // return;
1171 epc.pt = posSelf+m_qrot*m_parts[0].pos;
1172 epc.pt += n*(n*(pt-epc.pt));
1173 float cosa=n*(m_qrot*Vec3(0,0,1)), sina=sqrt_tpl(max(0.0f,1.0f-cosa*cosa));
1174 epc.radius = (m_size.x*cosa+(m_size.z+m_size.x*m_bUseCapsule)*sina)*1.05f;
1175 }
1176 epc.iPrim[1] = iPrim;
1177 m_pWorld->OnEvent(m_flags,&epc);
1178 m_bHadCollisions = 1;
1179 }
1180 }
1181
1182 void CLivingEntity::RegisterUnprojContact(const le_contact &unproj)
1183 {
1184 if (m_nContacts==m_nContactsAlloc) {
1185 le_contact *pContacts = m_pContacts;
1186 m_pContacts = new le_contact[m_nContactsAlloc+=4];
1187 NO_BUFFER_OVERRUN // cppcheck-suppress memsetClass
1188 memcpy(m_pContacts, pContacts, m_nContacts*sizeof(le_contact));
1189 delete[] pContacts;
1190 }
1191 NO_BUFFER_OVERRUN
1192 memcpy(m_pContacts+m_nContacts, &unproj, sizeof(unproj));
1193 m_pContacts[m_nContacts++].pSolverContact[0] = 0;
1194 AddCollider(unproj.pent);
1195 unproj.pent->AddCollider(this);
1196 }
1197
1198
1199 float CLivingEntity::GetMaxTimeStep(float time_interval)
1200 {
1201 if (m_timeStepPerformed > m_timeStepFull-0.001f)
1202 return time_interval;
1203 return min_safe(m_timeStepFull-m_timeStepPerformed,time_interval);
1204 }
1205
1206 Vec3 CLivingEntity::SyncWithGroundCollider(float time_interval)
1207 {
1208 int i; Vec3 newpos=m_pos;
1209 if (m_pLastGroundCollider && m_pLastGroundCollider->m_iSimClass==7)
1210 ReleaseGroundCollider();
1211 WriteLockCond lock(m_lockLiving,m_bStateReading^1);
1212 Vec3 velGround0 = m_velGround;
1213 m_velGround.zero();
1214
1215 if (m_pLastGroundCollider) {
1216 Vec3 coll_origin; quaternionf coll_q; float coll_scale;
1217 m_pLastGroundCollider->GetPartTransform(i=m_iLastGroundColliderPart, coll_origin,coll_q,coll_scale, this);
1218 newpos = coll_q*m_posLastGroundColl*coll_scale + coll_origin + m_qrot*Vec3(0,0,m_hPivot);
1219 const Vec3 diff = (newpos-m_pos);
1220 const float fMaxDist = m_maxVelGround * time_interval;
1221 if ( diff.len2() > sqr(fMaxDist) )
1222 newpos = m_pos+(diff).normalized()*(fMaxDist);
1223
1224 pe_status_dynamics sd;
1225 sd.ipart = i;
1226 m_pLastGroundCollider->GetStatus(&sd);
1227
1228 Matrix33 K; K.SetZero();
1229 RigidBody body(false), *pbody=m_pLastGroundCollider->GetRigidBodyTrans(&body, i, this, 0,true);
1230 pbody->GetContactMatrix(newpos-pbody->pos,K);
1231 Vec3 velGround = sd.v+(sd.w^newpos-sd.centerOfMass);
1232 if ((velGround-velGround0).len2() < m_gravity.len2()*sqr(time_interval*2.0f))
1233 m_velGround = velGround;
1234 else {
1235 Vec3 dP = (K+Matrix33(Diag33(Vec3(m_massinv)))).GetInverted()*(velGround-velGround0);
1236 m_velGround = velGround0+dP*m_massinv;
1237 if (m_velGround.len2() > velGround.len2())
1238 m_velGround = velGround;
1239 else {
1240 pe_action_impulse ai;
1241 ai.point = newpos;
1242 ai.impulse = -dP;
1243 ai.iApplyTime = 0;
1244 ai.ipart = i;
1245 m_pLastGroundCollider->Action(&ai);
1246 }
1247 }
1248
1249 if (m_velGround.len2()>sqr(m_maxVelGround))
1250 m_velGround.normalize() *= m_maxVelGround;
1251 }
1252 if (m_pWorld->m_bWorldStep==2)
1253 m_pos = newpos;
1254 return newpos;
1255 }
1256
1257
1258 void CLivingEntity::ComputeBBox(Vec3 *BBox, int flags)
1259 {
1260 CPhysicalEntity::ComputeBBox(BBox,flags);
1261 Vec3 pt = m_pNewCoords->pos-m_pNewCoords->q*Vec3(0,0,m_hPivot);
1262 BBox[0] = min(BBox[0],pt); BBox[1] = max(BBox[1],pt);
1263 if (sqr(m_pNewCoords->q.v.x)+sqr(m_pNewCoords->q.v.y)<0.001f) {
1264 Vec3 size = m_size+Vec3(0,0,m_bUseCapsule*m_size.x);
1265 BBox[0] = min(BBox[0],pt+Vec3(0,0,m_hCyl)-size); BBox[1] = max(BBox[1],pt+Vec3(0,0,m_hCyl)+size);
1266 }
1267 if (m_pHeadGeom) {
1268 pt = m_pNewCoords->pos+m_pNewCoords->q*Vec3(0,0,m_hHead+m_pHeadGeom->m_sphere.r-m_hPivot);
1269 BBox[0] = min(BBox[0],pt); BBox[1] = max(BBox[1],pt);
1270 }
1271 }
1272
1273 void CLivingEntity::UpdatePosition(const Vec3 &pos, const Vec3 *BBox, int bGridLocked)
1274 {
1275 WriteLockCond lock(m_lockUpdate,m_bStateReading^1);
1276 m_pos = pos;
1277
1278 if (m_nParts==1) {
1279 m_parts[0].BBox[0]=m_parts[0].pNewCoords->BBox[0]; m_parts[0].BBox[1]=m_parts[0].pNewCoords->BBox[1];
1280 m_parts[0].pNewCoords = (coord_block_BBox*)&m_parts[0].pos;
1281 m_BBox[0] = BBox[0]; m_BBox[1] = BBox[1];
1282 } else {
1283 for(int i=0;i<m_nParts;i++) m_parts[i].pNewCoords = (coord_block_BBox*)&m_parts[i].pos;
1284 ComputeBBox(m_BBox);
1285 }
1286 m_pWorld->UnlockGrid(this,-bGridLocked);
1287 }
1288
1289 void CLivingEntity::StartStep(float time_interval)
1290 {
1291 m_timeStepPerformed = 0;
1292 m_timeStepFull = time_interval;
1293 if (m_dtRequested>0) {
1294 if( time_interval > m_dtRequested )
1295 m_timeStepFull = m_dtRequested;
1296 m_dtRequested = 0;
1297 }
1298 m_nContacts = 0; m_bMoved = 0;
1299 }
1300
1301
1302 int CLivingEntity::Step(float time_interval)
1303 {
1304 if (time_interval<=0)
1305 return 1;
1306 float dt = m_timeStepFull-m_timeStepPerformed;
1307 time_interval = m_pWorld->m_bWorldStep==2 ? min(time_interval, dt) : dt;
1308 time_interval = max(time_interval, 0.001f);
1309
1310 const int iCaller = get_iCaller_int();
1311 int i,j,jmin,ipartMin,nents,ncont,bFlying,bWasFlying,bUnprojected,idmat,iPrim, bHasExtraParts=0,
1312 bHasFastPhys,icnt,nUnproj,bStaticUnproj,bDynUnproj,bMoving=0,nPrecompEnts=0,nPrecompParts=0, nUsedPartsCount=0,
1313 &nNoResponseAllocLE=m_pWorld->m_threadData[iCaller].nNoResponseAllocLE,
1314 &nPrecompEntsAlloc=m_pWorld->m_threadData[iCaller].nPrecompEntsAllocLE,&nPrecompPartsAlloc=m_pWorld->m_threadData[iCaller].nPrecompPartsAllocLE;
1315 const Quat oldQRot = m_qrot;
1316 Vec3 pos,vel,pos0,vel0,newpos,move(ZERO),nslope,ncontactHist[4],ncontact,ptcontact,ncontactSum,BBoxInner[2],velGround,axis,sz,heightAdj=Vec3(0.f);
1317 float movelen,tmin,vrel,movesum,kInertia;
1318 le_precomp_entity *&pPrecompEnts=m_pWorld->m_threadData[iCaller].pTmpPrecompEntsLE;
1319 le_precomp_part *&pPrecompParts=m_pWorld->m_threadData[iCaller].pTmpPrecompPartsLE;
1320 le_tmp_contact *&pNoResponseContactLE=m_pWorld->m_threadData[iCaller].pTmpNoResponseContactLE, *pNRC;
1321 le_contact unproj[8];
1322 CCylinderGeom CylinderGeomOuter,*pCyl[2];
1323 geom_world_data gwd[3];
1324 intersection_params ip;
1325 CPhysicalEntity **pentlist, *pentmin=NULL;
1326 geom_contact *pcontacts;
1327 pe_action_impulse ai;
1328 pe_status_dynamics sd;
1329 WriteLockCond lockMain(m_lockStep,m_bStateReading^1);
1330 ip.bNoAreaContacts = true;
1331
1332 IF (nNoResponseAllocLE==0, 0) pNoResponseContactLE=new le_tmp_contact[nNoResponseAllocLE=16];
1333
1334 if (m_timeForceInertia>0.0001f)
1335 kInertia = 6.0f;
1336 else if (m_kInertiaAccel && m_velRequested.len2()>0.1f)
1337 kInertia = m_kInertiaAccel;
1338 else
1339 kInertia = m_kInertia;
1340
1341 pos = m_pWorld->m_bWorldStep==2 ? m_pos : SyncWithGroundCollider(time_interval);
1342 vel0=vel = m_vel;
1343 bWasFlying = bFlying = m_bFlying;
1344 if (!m_bStateReading) {
1345 m_timeUseLowCap -= time_interval;
1346 m_timeSinceStanceChange += time_interval;
1347 m_timeSinceImpulseContact += time_interval;
1348 m_timeForceInertia = max(0.0f,m_timeForceInertia-time_interval);
1349 m_timeStepPerformed += time_interval;
1350 }
1351
1352 if (m_bActive &&
1353 (!(vel.len2()==0 && m_velRequested.len2()==0 && (!bFlying || m_gravity.len2()==0) && m_dhSpeed==0 && m_dhAcc==0) ||
1354 m_bActiveEnvironment || m_nslope.z<m_slopeSlide || m_velGround.len2()>0))
1355 {
1356 CRY_PROFILE_FUNCTION(PROFILE_PHYSICS );
1357 PHYS_ENTITY_PROFILER
1358
1359 m_bActiveEnvironment = 0;
1360 //m_nslope.Set(0,0,1);
1361 if (kInertia==0 && !bFlying && !m_bJumpRequested || m_pWorld->m_vars.bFlyMode) {
1362 vel = m_velRequested;
1363 vel -= m_nslope*(m_nslope*vel);
1364 }
1365 if (bFlying && !m_pWorld->m_vars.bFlyMode && m_kAirControl>0) {
1366 if (kInertia>0) {
1367 Vec3 velDelta = m_velRequested * (m_kInertia*time_interval*m_kAirControl);
1368 Vec3 velDiff = m_velRequested - vel;
1369
1370 const float kAirControlSelect = m_kAirControl-1.0f;
1371
1372 velDelta.x = (float)__fsel(-(velDiff.x * velDelta.x), 0.0f, velDelta.x);
1373 velDelta.y = (float)__fsel(-(velDiff.y * velDelta.y), 0.0f, velDelta.y);
1374 velDelta.z = (float)__fsel(-(velDiff.z * velDelta.z), 0.0f, velDelta.z);
1375
1376 velDelta.x = (float)__fsel(velDiff.x, min(velDiff.x, velDelta.x), max(velDiff.x, velDelta.x));
1377 velDelta.y = (float)__fsel(velDiff.y, min(velDiff.y, velDelta.y), max(velDiff.y, velDelta.y));
1378 velDelta.z = (float)__fsel(velDiff.z, min(velDiff.z, velDelta.z), max(velDiff.z, velDelta.z));
1379
1380 velDelta.y = (float)__fsel(kAirControlSelect, velDiff.y, velDelta.y);
1381 velDelta.x = (float)__fsel(kAirControlSelect, velDiff.x, velDelta.x);
1382
1383 vel += velDelta;
1384 } else if (m_gravity.len2()>0)
1385 vel = m_gravity*(vel*m_gravity-m_velRequested*m_gravity)/m_gravity.len2()+m_velRequested;
1386 else
1387 vel = m_velRequested;
1388 }
1389
1390 //filippo:m_forceFly is to let the game set a velocity no matter what is the status of the entity.
1391 if (m_forceFly)
1392 vel = m_velRequested;
1393 else if (bFlying && !m_bSwimming && !m_pWorld->m_vars.bFlyMode)
1394 move += m_gravity*sqr(time_interval)*0.5f;
1395
1396 if (vel.len2() > sqr(m_pWorld->m_vars.maxVelPlayers))
1397 vel.normalize() *= m_pWorld->m_vars.maxVelPlayers;
1398
1399 move += vel*time_interval;
1400
1401 m_forceFly = false;
1402 bUnprojected = 0;
1403 axis = m_qrot*Vec3(0,0,1);
1404 if (_isnan(move.len2())) //necessary? Is there any way that this can occur? - Rich S
1405 return 1;
1406 --m_bSquashed; m_bSquashed-=m_bSquashed>>31;
1407
1408 if (m_pWorld->m_vars.iCollisionMode!=0 && m_pWorld->m_vars.bFlyMode) { //Rich S - is bFlyMode a cheat? Can we disable in release?
1409 pos+=move; bFlying=1; m_hLatest=0; ReleaseGroundCollider();
1410 } else {
1411
1412 const float fSizeX = m_size.x;
1413 const float fSizeZ = m_size.z;
1414 movelen = move.len();
1415 const float fGap = (movelen+m_pWorld->m_vars.maxContactGapPlayer)*1.5f;
1416 const Vec3 posDiff = pos - m_pos, maxUpStep = axis*(m_hCyl-m_size.z-m_size.x*m_bUseCapsule);
1417 BBoxInner[0] = m_BBox[0]+(posDiff)-Vec3(fGap,fGap,fGap)+min(maxUpStep,Vec3(0));
1418 BBoxInner[1] = m_BBox[1]+(posDiff)+Vec3(fGap,fGap,fGap)+max(maxUpStep,Vec3(0));
1419 const float fGap2 = max(10.0f*time_interval,fGap); // adds a safety margin of m_size.x width
1420 const Vec3 BBoxOuter0 = m_BBox[0]+(posDiff)-Vec3(fGap2,fGap2,fGap2)+min(maxUpStep,Vec3(0));
1421 const Vec3 BBoxOuter1 = m_BBox[1]+(posDiff)+Vec3(fGap2,fGap2,fGap2)+max(maxUpStep,Vec3(0));
1422
1423 nents = m_pWorld->GetEntitiesAround(BBoxOuter0,BBoxOuter1,
1424 pentlist, m_collTypes|ent_independent|ent_triggers|ent_sort_by_mass, this, 0,iCaller);
1425
1426 if (m_vel.len2()>sqr(0.01f) || m_velRequested.len2()) for(i=0;i<m_nColliders;i++) if (m_pColliders[i]->HasConstraintContactsWith(this,constraint_inactive))
1427 m_pColliders[i]->Awake();
1428
1429 const float fMassInv = m_massinv;
1430 for(i=j=bHasFastPhys=0,vrel=0; i<nents; i++) if (!m_pForeignData || pentlist[i]->m_pForeignData!=m_pForeignData){
1431 Vec3 size = pentlist[i]->m_BBox[1]-pentlist[i]->m_BBox[0];
1432 int bFastPhys = 0;
1433 if (IgnoreCollision(m_collisionClass, pentlist[i]->m_collisionClass))
1434 continue;
1435 if (pentlist[i]->m_iSimClass==2) {
1436 if (pentlist[i]->m_flags & ref_small_and_fast)
1437 continue;
1438 else if (pentlist[i]->GetMassInv()*0.4f<fMassInv) {
1439 pentlist[i]->GetStatus(&sd);
1440 vrel = max(vrel,sd.v.len()+sd.w.len()*max(max(size.x,size.y),size.z));;
1441 bHasFastPhys |= (bFastPhys = isneg(fSizeX*0.2f-vrel*time_interval));
1442 }
1443 }
1444 if (!bFastPhys && !AABB_overlap(pentlist[i]->m_BBox,BBoxInner) && size.len2()>0)
1445 continue;
1446 idmat = pentlist[i]->GetType();
1447 if (idmat==PE_SOFT || idmat==PE_ROPE)
1448 pentlist[i]->Awake();
1449 else if (pentlist[i]->m_iSimClass<4 &&
1450 (idmat!=PE_LIVING && !pentlist[i]->IgnoreCollisionsWith(this,1) ||
1451 idmat==PE_LIVING && pentlist[i]->m_parts[0].flags&collider_flags &&
1452 pentlist[i]!=this && m_pLivingEntToIgnore!=pentlist[i] && ((CLivingEntity*)pentlist[i])->m_pLivingEntToIgnore!=this))
1453 {
1454 if (pentlist[i]->m_iSimClass==1 && m_timeSinceImpulseContact<0.2f && pentlist[i]->GetMassInv()>0) {
1455 int ipart; unsigned int flags;
1456 for(ipart=0,flags=0; ipart<pentlist[i]->m_nParts; ipart++)
1457 flags |= pentlist[i]->m_parts[ipart].flags;
1458 if (flags & collider_flags)
1459 pentlist[i]->Awake();
1460 }
1461 pentlist[j++] = pentlist[i];
1462 }
1463 }
1464 nents = j;
1465 pos0 = pos;
1466 bStaticUnproj = bDynUnproj = 0;
1467 newpos = pos+move;
1468
1469 IF (nents>nPrecompEntsAlloc, 0) delete[] pPrecompEnts, pPrecompEnts=new le_precomp_entity[nPrecompEntsAlloc=nents];
1470 for(i=0; i<nents; i++) {
1471 le_precomp_entity &ent = pPrecompEnts[nPrecompEnts++];
1472 ent.BBox[0]=pentlist[i]->m_BBox[0];
1473 ent.BBox[1]=pentlist[i]->m_BBox[1];
1474 ent.sz=(sz=pentlist[i]->m_BBox[1]-pentlist[i]->m_BBox[0]);
1475 ent.bCheckBBox=sz.len2()>0;
1476 ent.massinv=pentlist[i]->GetMassInv();
1477 ent.entType=pentlist[i]->GetType();
1478 ent.iSimClass=pentlist[i]->m_iSimClass;
1479 ent.pent=pentlist[i];
1480 ent.nParts=0;
1481 ent.ignoreCollisionsWith=pentlist[i]->IgnoreCollisionsWith(this,1);
1482 ent.iPartsBegin=nPrecompParts;
1483 ent.iPartsEnd=nPrecompParts+(nUsedPartsCount=pentlist[i]->GetUsedPartsCount(iCaller));
1484 IF (ent.iPartsEnd>nPrecompPartsAlloc, 0) {
1485 ReallocateList(pPrecompParts, nPrecompParts, ent.iPartsEnd+128);
1486 nPrecompPartsAlloc=ent.iPartsEnd+128;
1487 }
1488 for(int j1=0; j1<nUsedPartsCount; j1++) {
1489 le_precomp_part &part = pPrecompParts[nPrecompParts++];
1490 Vec3 *pBBox = pentlist[i]->GetPartBBox(part.ipart=j=pentlist[i]->GetUsedPart(iCaller,j1), part.BBox, this);
1491 part.BBox[0]=pBBox[0]; part.BBox[1]=pBBox[1];
1492 pentlist[i]->GetPartTransform(j, part.partoff,part.partrot,part.partscale, this);
1493 part.partflags=pentlist[i]->m_parts[j].flags;
1494 part.pgeom=pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom;
1495 part.surface_idx=pentlist[i]->m_parts[j].surface_idx;
1496 ent.nParts+=1-iszero(-j1>>31 & part.partflags & (int)collider_flags);
1497 }
1498 }
1499
1500 // first, check if we need unprojection in the initial position
1501 if (sqr(m_qrot.v.x)+sqr(m_qrot.v.y)<sqr(0.001f))
1502 gwd[0].R.SetIdentity();
1503 else
1504 gwd[0].R = Matrix33(m_qrot);
1505 gwd[0].centerOfMass = gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
1506 gwd[0].v.zero(); gwd[0].w.zero(); // since we check a static character against potentially moving environment here
1507 ip.vrel_min = fSizeX;
1508 ip.time_interval = time_interval*2;
1509 ip.maxUnproj = fSizeX*2.5f;
1510 ip.ptOutsidePivot[0] = gwd[0].offset;
1511 pCyl[0] = m_pCylinderGeom;
1512 if (bHasFastPhys) {
1513 cylinder cylOuter;
1514 cylOuter.r = fSizeX+min(fSizeX*1.5f,vrel*time_interval);
1515 cylOuter.hh = fSizeZ+min(fSizeZ,vrel*time_interval);
1516 cylOuter.center.zero();
1517 cylOuter.axis.Set(0,0,1);
1518 CylinderGeomOuter.CreateCylinder(&cylOuter);
1519 pCyl[1] = &CylinderGeomOuter;
1520 }
1521
1522 retry_without_ground_sync:
1523 if (m_parts[0].flagsCollider) {
1524 for(i=nUnproj=0,ncontactSum.zero();i<nents;++i)
1525 if (pPrecompEnts[i].entType!=PE_LIVING) {
1526 const float minv = pPrecompEnts[i].massinv;
1527 const int bHeavy = isneg(minv*0.4f-fMassInv);
1528 const int iSimClass = pPrecompEnts[i].iSimClass;
1529 CPhysicalEntity *const pent = pPrecompEnts[i].pent;
1530 int bFastPhys = 0;
1531 if (bHasFastPhys && bHeavy && iSimClass==2 && pent!=m_pLastGroundCollider) {
1532 pent->GetStatus(&sd);
1533 const Vec3 &sz0 = pPrecompEnts[i].sz;
1534 vrel = max(vrel,sd.v.len()+sd.w.len()*max(max(sz0.x,sz0.y),sz0.z));
1535 bFastPhys = isneg(fSizeX*0.2f-vrel*time_interval);
1536 gwd[1].v = sd.v; gwd[1].w = sd.w;
1537 gwd[1].centerOfMass = sd.centerOfMass;
1538 } else {
1539 gwd[1].v.zero(); gwd[1].w.zero();
1540 }
1541
1542 for(int iCyl=0; iCyl<bFastPhys+m_nParts; iCyl++) {
1543 int igwd,flagsCollider; IGeometry *pCurCyl;
1544 if (iCyl<=bFastPhys) {
1545 pCurCyl=pCyl[iCyl]; igwd=0; flagsCollider=collider_flags;
1546 } else {
1547 pCurCyl = m_parts[iCyl-bFastPhys].pPhysGeom->pGeom;
1548 gwd[igwd=2].R = Matrix33(m_qrot*m_parts[iCyl-bFastPhys].q);
1549 gwd[2].centerOfMass = gwd[2].offset = pos + m_qrot*m_parts[iCyl-bFastPhys].pos;
1550 gwd[2].scale = m_parts[iCyl-bFastPhys].scale;
1551 if (!((flagsCollider = m_parts[iCyl-bFastPhys].flagsCollider) & geom_colltype_solid))
1552 continue;
1553 bHasExtraParts = 1;
1554 }
1555 for(int j1=pPrecompEnts[i].iPartsBegin, bCheckBBox=pPrecompEnts[i].bCheckBBox; j1<pPrecompEnts[i].iPartsEnd; ++j1)
1556 if (pPrecompParts[j1].partflags & flagsCollider && (!bCheckBBox || AABB_overlap(BBoxInner,pPrecompParts[j1].BBox))) {
1557 gwd[1].R = Matrix33(pPrecompParts[j1].partrot);
1558 gwd[1].offset = pPrecompParts[j1].partoff;
1559 gwd[1].scale = pPrecompParts[j1].partscale;
1560 j = pPrecompParts[j1].ipart;
1561 //if (m_pWorld->m_pRenderer) m_pWorld->m_pRenderer->DrawGeometry(pent->m_parts[j].pPhysGeomProxy->pGeom, &gwd[1], 4);
1562
1563 if (icnt=pCurCyl->Intersect(pPrecompParts[j1].pgeom, gwd+igwd,gwd+1, &ip, pcontacts)) {
1564 const uint32 uFlags = m_flags;
1565 for(ncont=0; ncont<icnt-1 && pcontacts[ncont].dir*(pcontacts[ncont].pt-gwd[0].offset)>0; ncont++);
1566 if ((pos0-m_pos).len2()>sqr(0.001f)) {//m_pos-posncont==icnt) {
1567 gwd[0].offset+=m_pos-pos; move-=m_pos-pos; pos=m_pos;
1568 pos0=m_pos; goto retry_without_ground_sync; // if we made SyncWithGroundCollider and it caused intersections, roll it back
1569 //continue;
1570 }
1571 if (iCyl==0 && bHeavy && !(pPrecompParts[j1].partflags & geom_no_coll_response)) {
1572 const int bVeryHeavyFlags = (~(-isneg(minv*2-fMassInv))) | pent->m_flags;
1573 if (m_pWorld->m_bWorldStep==2) { // this means step induced by rigid bodies moving around
1574 // if the entity is rigid, store the contact
1575 int bPushOther;
1576 if (bPushOther = iSimClass>0 && minv>0) {// && pent->m_iGroup==m_pWorld->m_iCurGroup) {
1577 nUnproj = min(nUnproj+1,(int)(CRY_ARRAY_COUNT(unproj)));
1578 unproj[nUnproj-1].pent = pent;
1579 unproj[nUnproj-1].ipart = j;
1580 unproj[nUnproj-1].pt = pcontacts[ncont].pt;
1581 unproj[nUnproj-1].n = pcontacts[ncont].n;
1582 unproj[nUnproj-1].center = gwd[0].offset;
1583 unproj[nUnproj-1].penetration = pcontacts[ncont].t;
1584 } else if (!(bVeryHeavyFlags & pef_cannot_squash_players)) {
1585 m_bSquashed = min(5, m_bSquashed+5*isneg(ncontactSum*pcontacts[ncont].n+0.99f));
1586 if (iSimClass>0)
1587 ncontactSum = pcontacts[ncont].n;
1588 }
1589
1590 // check previous contacts from this frame, register in entity if there are conflicts
1591 for(icnt=0; icnt<nUnproj-bPushOther; icnt++) {
1592 const float fUnprojDotContact = unproj[icnt].n*pcontacts[ncont].n;
1593 if (fUnprojDotContact<0) {
1594 RegisterUnprojContact(unproj[icnt]);
1595 if (bPushOther)
1596 RegisterUnprojContact(unproj[nUnproj-1]);
1597 if (!((bVeryHeavyFlags|unproj[icnt].pent->m_flags) & pef_cannot_squash_players))
1598 m_bSquashed = min(5, m_bSquashed+5*isneg(max(5*minv-fMassInv, fUnprojDotContact+0.99f)));
1599 }
1600 }
1601 } else if (!(bVeryHeavyFlags & pef_cannot_squash_players)) {
1602 m_bSquashed = min(5, m_bSquashed+5*isneg(ncontactSum*pcontacts[ncont].n+0.99f));
1603 if (iSimClass>0)
1604 ncontactSum = pcontacts[ncont].n;
1605 }
1606
1607 if (uFlags & pef_pushable_by_players) {
1608 (minv==0 ? bStaticUnproj:bDynUnproj)++;
1609 Vec3 offs = pcontacts[ncont].dir*(pcontacts[ncont].t+m_pWorld->m_vars.maxContactGapPlayer);
1610 pos += offs; gwd[0].offset += offs;
1611 bUnprojected = 1;
1612 if (pcontacts[ncont].t>m_size.x)
1613 ip.ptOutsidePivot[0].Set(1E11f,1E11f,1E11f);
1614 }
1615 }
1616
1617 if (iSimClass==2) {
1618 Matrix33 K(ZERO);
1619 if (uFlags & pef_pushable_by_players)
1620 K += Matrix33(Diag33(m_massinv));
1621 int bPushOther = (uFlags & lef_push_objects) && (pent->m_flags & pef_pushable_by_players);
1622 bPushOther |= iszero(iSimClass-2);
1623 bPushOther &= iszero((int)(INT_PTR)pent-(int)(INT_PTR)m_pLastGroundCollider)^1;
1624 if (bPushOther) {
1625 RigidBody body(false), *pbody=pent->GetRigidBodyTrans(&body, j, this, 0,true);
1626 pbody->GetContactMatrix(pcontacts[ncont].center-pbody->pos, K);
1627 }
1628 else if (!(uFlags & pef_pushable_by_players))
1629 continue;
1630 pcontacts[ncont].center -= pcontacts[ncont].dir*pcontacts[ncont].t;
1631 ncontact = -pcontacts[ncont].n;//(gwd[0].offset-pcontacts[ncont].center).normalized();
1632 if (fabs_tpl(ncontact.z)<0.5f) {
1633 ncontact.z=0; ncontact.normalize();
1634 }
1635 RigidBody body(false), *pbody = pent->GetRigidBodyTrans(&body,j,this);
1636 vrel = ncontact*(pbody->v+(pbody->w^pcontacts[ncont].center-pbody->pos)-vel-m_velGround);
1637 if (iCyl==0 || fabs_tpl(vrel)*time_interval>m_size.x*0.2f) {
1638 vrel = max(0.0f,vrel-ncontact*(vel+m_velGround));
1639 float imp=vrel/max(1e-6f,ncontact*K*ncontact);
1640 ai.impulse = ncontact*imp;
1641 ai.point = pcontacts[ncont].center; ai.ipart = 0;
1642 if (ai.impulse.len2()*sqr(fMassInv) > max(1.0f,sqr(vrel)))
1643 ai.impulse.normalize() *= fabs_tpl(vrel)*m_mass*0.5f;
1644 if ((uFlags & (pef_pushable_by_players|geom_no_coll_response)) == pef_pushable_by_players)
1645 vel += ai.impulse*fMassInv;
1646 /*if (vel.z>-5) {
1647 vel.z = max(0.0f, vel.z); vel.z += ai.impulse.len()*fMassInv*0.1f;
1648 }*/
1649 bFlying = 1; m_timeFlying = 0;
1650 if (m_kInertia==0)
1651 m_timeForceInertia = m_timeImpulseRecover;
1652 if (bPushOther) {
1653 if (fabs_tpl(pcontacts[ncont].dir*axis)<0.7f)
1654 ai.impulse -= axis*(axis*ai.impulse); // ignore vertical component - might be semi-random depending on cylinder intersection
1655 ai.impulse.Flip(); ai.ipart = j;
1656 //if (pent->GetType()<PE_LIVING)
1657 // ai.impulse *= 0.2f;
1658 pent->Action(&ai,1);
1659 m_timeSinceImpulseContact = 0;
1660 }
1661 idmat = pPrecompParts[j1].surface_idx&pcontacts[ncont].id[1]>>31 | max(pcontacts[ncont].id[1],0);
1662 RegisterContact(pos,pcontacts[ncont].pt,ncontact,pent,j,idmat,imp,0,pcontacts[ncont].iPrim[1]);
1663 }
1664 }
1665 //break;
1666 }
1667 }
1668 }
1669 }
1670 }
1671 if (bStaticUnproj && bDynUnproj && m_bSquashed) {
1672 pos = pos0; // disable unprojection if we are being squashed
1673 bStaticUnproj = bDynUnproj = 0;
1674 }
1675 else if (bStaticUnproj+bDynUnproj>0) {
1676 newpos = pos+move;
1677 }
1678
1679 float h = ShootRayDown(pPrecompEnts,nents,pPrecompParts,newpos,nslope);
1680 float hcur = newpos*axis-m_hPivot;
1681 const float fAxisDotSlope = axis*nslope;
1682 if (fAxisDotSlope>m_slopeFall &&
1683 (hcur<h && hcur>h-(m_hCyl-m_size.z)*1.01f ||
1684 hcur>h && sqr_signed(hcur-h)<vel.len2()*sqr(time_interval) && !bFlying && !m_bJumpRequested && !m_bSwimming))
1685 {
1686 if (h>hcur && m_nslope*axis<m_slopeSlide && m_nslope*nslope<m_slopeSlide &&
1687 fAxisDotSlope<m_slopeSlide && m_velRequested.len2()==0)
1688 {
1689 newpos = pos; vel.zero();
1690 } else
1691 newpos += (heightAdj = axis*(h+m_hPivot-newpos*axis));
1692 move = newpos-pos; movelen = move.len();
1693 }
1694 pos0 = pos;
1695 if (m_bJumpRequested)
1696 AddLegsImpulse(-vel,m_nslope,true);
1697 m_bJumpRequested = 0;
1698 m_bStuck = 0; ncontactSum.zero();
1699
1700 if (movelen>m_size.x*1E-4f && m_parts[0].flagsCollider!=0) {
1701 ip.bSweepTest = true;
1702 gwd[0].v = move/movelen;
1703 int iter = 0;
1704 float move0 = movelen; movesum = 0;
1705 pe_player_dimensions pd;
1706
1707 do {
1708 float tlim = 0.0f;
1709 gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
1710 ip.time_interval = movelen+m_pWorld->m_vars.maxContactGapPlayer; tmin = ip.time_interval*2; iPrim = -1;
1711 pNRC = pNoResponseContactLE; pNRC->pent = NULL; pNRC->tmin = tmin;
1712 for(i=0; i<nents; ++i) {
1713 if (pPrecompEnts[i].entType!=PE_LIVING ||
1714 pPrecompEnts[i].nParts+bHasExtraParts > 0 ||
1715 max(sqr(m_qrot.v.x)+sqr(m_qrot.v.y),sqr(pPrecompEnts[i].pent->m_qrot.v.x)+sqr(pPrecompEnts[i].pent->m_qrot.v.y))>0.001f)
1716 {
1717 CPhysicalEntity *const pent=pPrecompEnts[i].pent;
1718 int bCheckBBox=pPrecompEnts[i].bCheckBBox;
1719 for(int j1=pPrecompEnts[i].iPartsBegin; j1<pPrecompEnts[i].iPartsEnd; ++j1) {
1720 const uint32 uPartFlags = pPrecompParts[j1].partflags;
1721 if (uPartFlags & (collider_flags|geom_log_interactions) &&
1722 (!bCheckBBox || AABB_overlap(BBoxInner,pPrecompParts[j1].BBox)))
1723 {
1724 if (uPartFlags & geom_log_interactions) {
1725 EventPhysBBoxOverlap event;
1726 event.pEntity[0]=this; event.pForeignData[0]=m_pForeignData; event.iForeignData[0]=m_iForeignData;
1727 event.pEntity[1]=pent; event.pForeignData[1]=pent->m_pForeignData; event.iForeignData[1]=pent->m_iForeignData;
1728 m_pWorld->OnEvent(m_flags, &event);
1729 if (!(uPartFlags & collider_flags))
1730 continue;
1731 }
1732 gwd[1].R = Matrix33(pPrecompParts[j1].partrot);
1733 gwd[1].offset = pPrecompParts[j1].partoff;
1734 gwd[1].scale = pPrecompParts[j1].partscale;
1735
1736 int ipart = 0;
1737 if((ncont = m_pCylinderGeom->Intersect(pPrecompParts[j1].pgeom, gwd,gwd+1, &ip, pcontacts)))
1738 got_contact:
1739 if (pcontacts[ncont-1].t<tmin && pcontacts[ncont-1].n*gwd[0].v>0) {
1740 if (!((pPrecompParts[j1].partflags | m_parts[ipart].flags)&geom_no_coll_response)) {
1741 // Solid contact
1742 tmin = pcontacts[ncont-1].t; tlim=0.0f; ncontact = -pcontacts[ncont-1].n; ptcontact = pcontacts[ncont-1].pt; iPrim = pcontacts[ncont-1].iPrim[1];
1743 pentmin=pent; jmin=pPrecompParts[j1].ipart; idmat=pPrecompParts[j1].surface_idx&pcontacts[ncont-1].id[1]>>31 | max(pcontacts[ncont-1].id[1],0); ipartMin=ipart;
1744 } else if (pcontacts[ncont-1].t<pNRC->tmin) {
1745 // Non-response contact
1746 pNRC->pent = pent; pNRC->tmin = pcontacts[ncont-1].t;
1747 pNRC->ptcontact = pcontacts[ncont-1].pt; pNRC->ncontact = -pcontacts[ncont-1].n;
1748 pNRC->ipart = pPrecompParts[j1].ipart; pNRC->iPrim = pcontacts[ncont-1].iPrim[1];
1749 pNRC->idmat = pPrecompParts[j1].surface_idx&pcontacts[ncont-1].id[1]>>31 | max(pcontacts[ncont-1].id[1],0);
1750 }
1751 }
1752 for(ipart++; ipart<(m_nParts&-bHasExtraParts); ipart++) if (m_parts[ipart].flagsCollider & uPartFlags) {
1753 gwd[2].R = Matrix33(m_qrot*m_parts[ipart].q);
1754 gwd[2].centerOfMass = gwd[2].offset = pos + m_qrot*m_parts[ipart].pos;
1755 gwd[2].scale = m_parts[ipart].scale; gwd[2].v = gwd[0].v;
1756 if((ncont = m_parts[ipart].pPhysGeomProxy->pGeom->Intersect(pPrecompParts[j1].pgeom, gwd+2,gwd+1, &ip, pcontacts)))
1757 goto got_contact;
1758 }
1759 }
1760 }
1761 } else {
1762 CPhysicalEntity *const pent=pPrecompEnts[i].pent;
1763 pent->GetParams(&pd);
1764 Vec2 dorigin,ncoll,move2d=(Vec2)gwd[0].v;
1765 dorigin = Vec2(pos-pent->m_pos);
1766 float kb=dorigin*move2d, kc=len2(dorigin)-sqr(m_size.x+pd.sizeCollider.x), kd=kb*kb-kc, zup0,zdown0,zup1,zdown1;
1767 if (kd>=0) {
1768 zup0 = (zdown0 = pos.z-m_hPivot)+m_hCyl+m_size.z+m_size.x*m_bUseCapsule;
1769 zdown0 = max(zdown0, zup0-(m_size.x*m_bUseCapsule+m_size.z)*2);
1770 zup1 = (zdown1 = pent->m_pos.z-pd.heightPivot)+pd.heightCollider+pd.sizeCollider.z+pd.sizeCollider.x*pd.bUseCapsule;
1771 zdown1 = max(zdown1, zup1-(pd.sizeCollider.x*pd.bUseCapsule+pd.sizeCollider.z)*2);
1772 kd=sqrt_tpl(kd);
1773 float tfirst=-kb+kd; ncoll = Vec2(pos+gwd[0].v*tfirst-pent->m_pos);
1774 bool bSideContact = min(zup0+gwd[0].v.z*tfirst,zup1)>max(zdown0+gwd[0].v.z*tfirst,zdown1);
1775 if (tfirst>-m_size.x && tfirst<tmin && ncoll*move2d>=0 && bSideContact)
1776 continue; // if entities separate during this timestep, ignore other collisions
1777 tfirst=-kb-kd; ncoll = Vec2(pos+gwd[0].v*tfirst-pent->m_pos);
1778 /*if (tfirst<-m_size.x || min(zup0+gwd[0].v.z*tfirst,zup1)<max(zdown0+gwd[0].v.z*tfirst,zdown1) || ncoll*move2d>=0) {
1779 tfirst=-kb+kd; ncoll = Vec2(pos+gwd[0].v*tfirst-pent->m_pos);
1780 }*/
1781 if (tfirst>-m_size.x && tfirst<tmin && ncoll*move2d<0 && bSideContact) {
1782 tlim = (tmin = tfirst)*iszero(iter); ncontact.Set(ncoll.x,ncoll.y,0).normalize(); ptcontact = pos+gwd[0].v*tfirst-ncontact*m_size.x;
1783 pentmin=pent; jmin=0; idmat=m_surface_idx; iPrim=-1; ipartMin=0;
1784 }
1785 // also check for cap-cap contact
1786 if (fabs_tpl(gwd[0].v.z)>m_size.z*1E-5f) {
1787 int nSignZ = sgnnz(gwd[0].v.z); //Could be float?
1788 zup0 = pos.z-m_hPivot+m_hCyl+(m_size.z+m_size.x*m_bUseCapsule)*nSignZ;
1789 zdown1 = pent->m_pos.z-pd.heightPivot+pd.heightCollider-(pd.sizeCollider.z+pd.sizeCollider.x*pd.bUseCapsule)*nSignZ;
1790 tfirst = zdown1-zup0;
1791 if (inrange(tfirst,-m_size.x*gwd[0].v.z,tmin*gwd[0].v.z) &&
1792 len2(dorigin*gwd[0].v.z+move2d*tfirst)<sqr((m_size.x+pd.sizeCollider.x)*gwd[0].v.z))
1793 {
1794 tmin = tfirst/gwd[0].v.z; ncontact.Set(0,0,-nSignZ); (ptcontact = pos+gwd[0].v*tfirst).z += m_size.z*nSignZ; //?
1795 pentmin=pent; jmin=-1; idmat=m_surface_idx; iPrim=-1; ipartMin=0;
1796 }
1797 }
1798 }
1799 }
1800 if (pNRC->pent) {
1801 IF (pNRC>=(pNoResponseContactLE+nNoResponseAllocLE-1), 0) {
1802 delete [] pNoResponseContactLE; pNoResponseContactLE=new le_tmp_contact[nNoResponseAllocLE+16];
1803 pNRC=pNoResponseContactLE+nNoResponseAllocLE-1; nNoResponseAllocLE+=16;
1804 }
1805 ++pNRC; pNRC->pent=NULL; pNRC->tmin=ip.time_interval*2;
1806 }
1807 }
1808
1809 if (tmin<=ip.time_interval) {
1810 tmin = max(tlim,tmin-m_pWorld->m_vars.maxContactGapPlayer);
1811 pos += gwd[0].v*tmin;
1812 static const float g_cosSlide=cos_tpl(0.3f), g_sinSlide=sin_tpl(0.3f);
1813 /*if (bFlying) {
1814 if ((ncontact*axis)*(1-m_bSwimming)>g_cosSlide)
1815 ncontact = axis*g_cosSlide + (pos-ptcontact-axis*(axis*(pos-ptcontact))).normalized()*g_sinSlide;
1816 } else */
1817 if (!bFlying && inrange(ncontact*axis, 0.85f,-0.1f) && (unsigned int)pentmin->m_iSimClass-1u<2u &&
1818 pentmin->GetMassInv()>m_massinv*0.25f)
1819 ncontact.z=0, ncontact.normalize();
1820 int bPush = pentmin->m_iSimClass>0 || isneg(min(m_slopeClimb-ncontact*axis, ncontact*axis+m_slopeFall));
1821 int bUnmovable = isneg(-pentmin->m_iSimClass>>31 & ~(-((int)m_flags & pef_pushable_by_players)>>31));
1822 bPush &= bUnmovable^1;
1823 {
1824 Matrix33 K(ZERO);
1825 int bPushOther = (m_flags & (pentmin->m_iSimClass==3 ? lef_push_players : lef_push_objects)) &&
1826 (pentmin->m_flags & pef_pushable_by_players) &&
1827 (pentmin->m_iSimClass | m_pWorld->m_vars.bPlayersCanBreak | pentmin->m_flags & pef_players_can_break);
1828 bPushOther &= iszero((int)(INT_PTR)pentmin-(int)(INT_PTR)m_pLastGroundCollider)^1;
1829 bPushOther |= bUnmovable;
1830 if (bPushOther) {
1831 RigidBody body(false), *pbody=pentmin->GetRigidBodyTrans(&body, jmin, this, 0,true);
1832 pbody->GetContactMatrix(ptcontact-pbody->pos, K);
1833 }
1834 if (!bPushOther || /*pentmin->m_iSimClass-3 | */m_flags & pef_pushable_by_players)
1835 K += Matrix33(Diag33(m_massinv));
1836 else
1837 bPush = 0;
1838 vrel = ncontact*(vel+m_velGround); //(ncontact*gwd[0].v)*vel.len();
1839 ai.impulse = ncontact;
1840 if (pentmin->m_iSimClass==3) {
1841 // make the player slide off when standing on other players
1842 vrel -= ncontact*((CLivingEntity*)pentmin)->m_vel;
1843 if (ncontact*axis > 0.95f) {
1844 ai.impulse += (pos-pentmin->m_pos-axis*(axis*(pos-pentmin->m_pos))).normalized();
1845 if (inrange(vrel,-1.0f,1.0f))
1846 vrel = -1.0f;
1847 }
1848 } else {
1849 RigidBody body(false), *pbody = pentmin->GetRigidBodyTrans(&body,jmin,this);
1850 vrel -= ncontact*(pbody->v+(pbody->w^ptcontact-pbody->pos));
1851 }
1852 vrel = min(0.0f, vrel);
1853 float imp = -vrel*1.01f/max(1e-6f,ncontact*K*ncontact);
1854 jmin -= jmin>>31;
1855 if (bPush || m_flags & lef_report_sliding_contacts || pentmin->m_parts[jmin].flags & geom_manually_breakable)
1856 RegisterContact(pos,ptcontact,ncontact,pentmin,jmin,idmat,imp*bPush,0,iPrim,ipartMin);
1857 ai.impulse *= imp;
1858 ai.point = ptcontact; ai.ipart = 0;
1859 if (ai.impulse.len2()*sqr(m_massinv) > max(1.0f,sqr(vrel)))
1860 ai.impulse.normalize() *= fabs_tpl(vrel)*m_mass*0.5f;
1861 if (bPush) {
1862 vel += ai.impulse*m_massinv;
1863 if (m_kInertia==0 && (pentmin->m_iSimClass-3 | m_flags & pef_pushable_by_players))
1864 m_timeForceInertia = m_timeImpulseRecover;
1865 }
1866 if (bPushOther) {
1867 ai.impulse.Flip(); ai.ipart = jmin;
1868 if (fabs_tpl(ncontact*axis)<0.7f)
1869 ai.impulse -= axis*(axis*ai.impulse); // ignore vertical component - might be semi-random depending on cylinder intersection
1870 ai.iApplyTime = isneg(pentmin->m_iSimClass-3)<<1;
1871 if (pentmin->GetType()<PE_LIVING)
1872 ai.impulse *= 0.2f;
1873 pentmin->Action(&ai);
1874 if (pentmin->m_iSimClass<3)
1875 m_timeSinceImpulseContact = 0;
1876 }
1877 }
1878 movelen -= tmin; movesum += tmin;
1879 for(i=0;i<iter && ncontactHist[i]*ncontact<0.95f;i++);
1880 if (i==iter)
1881 ncontactSum += ncontact;
1882 ncontactHist[iter] = ncontact;
1883 if (iter==1 && movesum==0.0f) {
1884 ncontact = (ncontactHist[0]^ncontactHist[1]).normalized();
1885 gwd[0].v = ncontact*(gwd[0].v*ncontact);
1886 } else {
1887 gwd[0].v -= ncontact*(gwd[0].v*ncontact);
1888 gwd[0].v = gwd[0].v*0.9998f+ncontact*(gwd[0].v.len()*0.02f);
1889 }
1890 tmin = gwd[0].v.len(); movelen*=tmin;
1891 if (tmin>0) gwd[0].v/=tmin;
1892 movelen = (float)__fsel(gwd[0].v*move, movelen, 0.0f);
1893 } else {
1894 pos += gwd[0].v*movelen; movesum += movelen; /* exit do-loop */ iter=1000;
1895 }
1896 for (le_tmp_contact* c=pNoResponseContactLE; c<=pNRC && c->pent; c++)
1897 if (c->tmin<tmin)
1898 RegisterContact(pos,c->ptcontact,c->ncontact,c->pent,c->ipart,c->idmat,0.f,0,c->iPrim);
1899 } while(movelen>m_pWorld->m_vars.maxContactGapPlayer*0.1f && ++iter<3);
1900
1901 const float fContactSumLenSq = ncontactSum.len2();
1902 if(movesum<move0*0.001f && (sqr_signed(ncontactSum.z)>sqr(0.4f)*fContactSumLenSq || fContactSumLenSq<0.6f))
1903 m_bStuck = 1;
1904
1905 if (m_parts[0].flagsCollider!=0 && (bUnprojected || !(m_flags & lef_loosen_stuck_checks))) {
1906 ip.bSweepTest = false;
1907 gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
1908 gwd[0].v = -axis;
1909 ip.bStopAtFirstTri = true; ip.bNoBorder = true; ip.time_interval = m_size.z*10;
1910 for(i=0; i<nents; ++i)
1911 if (pPrecompEnts[i].iSimClass==0) {
1912 CPhysicalEntity *const pent = pPrecompEnts[i].pent;
1913 for(int j1=pPrecompEnts[i].iPartsBegin; j1<pPrecompEnts[i].iPartsEnd; ++j1)
1914 if (pPrecompParts[j1].partflags & collider_flags && !(pPrecompParts[j1].partflags & geom_no_coll_response)) {
1915 gwd[1].R = Matrix33(pPrecompParts[j1].partrot);
1916 gwd[1].offset = pPrecompParts[j1].partoff;
1917 gwd[1].scale = pPrecompParts[j1].partscale;
1918 if(m_pCylinderGeom->Intersect(pPrecompParts[j1].pgeom, gwd,gwd+1, &ip, pcontacts)) {
1919 if (pcontacts->t>m_pWorld->m_vars.maxContactGapPlayer)
1920 vel.zero(),m_bStuck=1;
1921 pos = pos0; m_timeUseLowCap=1.0f;
1922 goto nomove;
1923 }
1924 }
1925 } nomove:;
1926 }
1927 } else
1928 pos += move;
1929
1930 if (!m_pLastGroundCollider)// || m_pLastGroundCollider->GetMassInv()>m_massinv)
1931 velGround.zero();
1932 else
1933 velGround = m_velGround;
1934 m_hLatest = h = ShootRayDown(pPrecompEnts, nents, pPrecompParts, pos,nslope, time_interval,false,true);
1935 if (nslope*axis<0.087f)
1936 nslope = m_nslope;
1937 else {
1938 WriteLockCond lock(m_lockLiving,m_bStateReading^1);
1939 m_nslope = nslope;
1940 }
1941 if (m_pLastGroundCollider) {
1942 RegisterContact(newpos,newpos,m_qrot*Vec3(0,0,1),m_pLastGroundCollider,m_iLastGroundColliderPart,m_lastGroundSurfaceIdx,0,1,m_lastGroundPrim);
1943 if (m_pLastGroundCollider->m_iSimClass==0)
1944 ReleaseGroundCollider();
1945 }
1946
1947 if (bFlying)
1948 m_timeFlying += time_interval;
1949 int bGroundContact = isneg(max(pos*axis-m_hPivot-(h+m_groundContactEps), m_slopeFall-nslope*axis));
1950 if (!bGroundContact)
1951 ReleaseGroundCollider();
1952
1953 bFlying = m_pWorld->m_vars.bFlyMode || m_gravity*axis>0 || m_bSwimming || ((bGroundContact|m_bStuck)^1);
1954 m_bActiveEnvironment = m_bStuck;
1955
1956 if (bFlying)
1957 Step_HandleFlying(vel, velGround, bWasFlying, heightAdj, kInertia, time_interval);
1958 else {
1959 if (bWasFlying)
1960 Step_HandleWasFlying(vel, bFlying, axis, bGroundContact);
1961
1962 Vec3 velReq = m_velRequested,g;
1963 if (!m_bSwimming) velReq -= m_nslope*(velReq*m_nslope);
1964 if (kInertia * time_interval > 1.0f) kInertia = 1.0f/time_interval;
1965 Vec3 last_force = (velReq-vel)*kInertia;
1966 const float axisSlope = m_nslope*axis;
1967 if (axisSlope<m_slopeSlide && !m_bSwimming) {
1968 g = m_gravity;
1969 last_force += g-m_nslope*(g*m_nslope);
1970 }
1971
1972 const Vec3 velIncLastForce = vel + (last_force*time_interval);
1973 if (velIncLastForce*vel<0 && velIncLastForce*m_velRequested<=0)
1974 vel.zero();
1975 else
1976 vel = velIncLastForce;
1977
1978 if (axisSlope<m_slopeClimb) {
1979 const float axisVel = vel*axis;
1980 if (axisVel>0 && last_force*axis>0)
1981 vel -= axis*(axisVel);
1982 if ((pos-pos0)*axis > m_size.z*0.001f)
1983 vel -= axis*(axis*vel);
1984 }
1985 if (axisSlope<m_slopeFall && !m_bStuck) {
1986 bFlying=1; vel += m_nslope-axis*(axisSlope);
1987 }
1988 if (m_velRequested.len2()==0 && vel.len2()<0.001f || vel.len2()<0.0001f)
1989 vel.zero();
1990 }
1991
1992 if (!bFlying)
1993 m_timeFlying = 0;
1994
1995 if (m_flags & lef_snap_velocities)
1996 vel = DecodeVec6b(EncodeVec6b(vel));
1997
1998 if (!m_bStateReading) {
1999 float dh;
2000 if (!bFlying && (dh=(pos-pos0)*axis)>m_size.z*0.01f) {
2001 m_dhSpeed = max(m_dhSpeed, dh/m_stablehTime);
2002 m_dh += dh;
2003 m_stablehTime = 0;
2004 } else
2005 dh = 0;
2006 m_stablehTime = min(m_stablehTime+time_interval,0.5f);
2007
2008 m_dhSpeed += m_dhAcc*time_interval;
2009 if (m_dhAcc==0 && m_dh*m_dhSpeed<0 || m_dh*m_dhAcc<0 || m_dh*(m_dh-m_dhSpeed*time_interval)<0)
2010 m_dh = m_dhSpeed = m_dhAcc = 0;
2011 else
2012 m_dh -= m_dhSpeed*time_interval;
2013 }
2014
2015 if (m_pHeadGeom) {
2016 ip.bSweepTest = true;
2017 gwd[0].offset = pos + gwd[0].R*m_parts[0].pos;
2018 gwd[0].v = axis;
2019 tmin = ip.time_interval = m_hHead-m_hCyl-min(m_dh,0.0f);
2020 for(i=0;i<nents;i++) if (pentlist[i]->m_iSimClass==0) {//pentlist[i]->GetType()!=PE_LIVING && pentlist[i]->GetMassInv()*0.4f<m_massinv) {
2021 for(int j1=0;j1<pentlist[i]->GetUsedPartsCount(iCaller);j1++)
2022 if (pentlist[i]->m_parts[j=pentlist[i]->GetUsedPart(iCaller,j1)].flags & collider_flags) {
2023 pentlist[i]->GetPartTransform(j, gwd[1].offset,gwd[1].R,gwd[1].scale, this);
2024 if(m_pHeadGeom->Intersect(pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom, gwd,gwd+1, &ip, pcontacts))
2025 tmin = min(tmin,(float)pcontacts[0].t);
2026 }
2027 }
2028 if (m_dh<ip.time_interval+min(m_dh,0.0f)-tmin || fabs_tpl(m_dhSpeed)+fabs_tpl(m_dhAcc)==0)
2029 m_dh = ip.time_interval+min(m_dh,0.0f)-tmin;
2030 }
2031 }
2032
2033 coord_block coord;
2034 coord.pos = pos; coord.q = m_qrot;
2035 m_pNewCoords = &coord;
2036 coord_block_BBox partCoord;
2037 partCoord.pos = m_parts[0].pos; partCoord.q = m_parts[0].q;
2038 partCoord.scale = m_parts[0].scale; m_parts[0].pNewCoords = &partCoord;
2039 ComputeBBox(BBoxInner, update_part_bboxes & m_nParts-2>>31);
2040 int locked = m_pWorld->RepositionEntity(this,1,BBoxInner);
2041 pos = coord.pos;
2042 m_pNewCoords = (coord_block*)&m_pos;
2043 UpdatePosition(pos,BBoxInner, locked);
2044 bMoving = 1;
2045 } else if (!m_bActive) {
2046 m_pos = pos;
2047 if (m_velRequested.len2()>0) {
2048 m_pos += m_velRequested*time_interval;
2049 m_BBox[0] += m_velRequested*time_interval; m_BBox[1] += m_velRequested*time_interval;
2050 m_pWorld->RepositionEntity(this,1|8);
2051 bMoving = 1;
2052 }
2053 if (m_bReleaseGroundColliderWhenNotActive!=0)
2054 ReleaseGroundCollider();
2055 }
2056
2057 { WriteLockCond lock(m_lockLiving,m_bStateReading^1);
2058 //if (m_pWorld->m_vars.bMultiplayer)
2059 // m_pos = CompressPos(m_pos);
2060
2061 m_deltaV = (m_vel - vel0);
2062 m_vel = vel+m_vel-vel0;
2063 m_bFlying = bFlying;
2064 m_deltaQRot = m_qrot * !oldQRot;
2065
2066 m_timeSmooth = (float)__fsel(-time_interval, m_timeSmooth, time_interval);
2067 if (m_pWorld->m_bUpdateOnlyFlagged) {
2068 m_deltaPos = m_posLocal-m_pos;
2069 if (m_deltaPos.len2()<sqr(0.01f) || m_deltaPos.len2()>sqr(2.0f))
2070 m_deltaPos.zero();
2071 }
2072 if (m_pBody)
2073 if (!m_nColliders) {
2074 delete m_pBody; m_pBody=0;
2075 } else {
2076 m_pBody->pos=m_pos+m_qrot*Vec3(0,0,m_hCyl); m_pBody->q=m_qrot;
2077 m_pBody->P=(m_pBody->v=m_vel)*(m_pBody->M=m_mass);
2078 m_pBody->Minv=m_massinv;
2079 if (m_pWorld->m_timePhysics > m_timeRotChanged+0.05f)
2080 m_pBody->w.zero(), m_pBody->L.zero();
2081 /*quaternionf dq = m_history[m_iHist].q*!m_history[m_iHist-3&m_szHistory-1].q;
2082 float dt=0; for(i=0; i<4; i++)
2083 dt += m_history[m_iHist-i&m_szHistory-1].dt;
2084 if (inrange(dt, 0.0f,1.0f)) {
2085 if (dq.v.len2()<sqr(0.05f))
2086 m_pBody->w = dq.v*(2/dt);
2087 else
2088 m_pBody->w = dq.v.normalized()*(acos_tpl(dq.w)*2/dt);
2089 }*/
2090 }
2091 }
2092
2093 if (!m_bStateReading) {
2094 if( bMoving ) {
2095 Vec3 gravity; MARK_UNUSED gravity;
2096 pe_params_buoyancy pb;
2097 m_pWorld->CheckAreas(this,gravity,&pb,0);
2098 if (!is_unused(gravity))
2099 m_gravity = gravity;
2100
2101 if (m_pWorld->m_pWaterMan)
2102 m_pWorld->m_pWaterMan->OnWaterInteraction(this);
2103 }
2104
2105 if (m_flags & (pef_monitor_poststep | pef_log_poststep)) {
2106 EventPhysPostStep epps; InitEvent(&epps,this,iCaller);
2107 epps.dt=time_interval; epps.pos=m_pos; epps.q=m_qrot; epps.idStep=m_pWorld->m_idStep;
2108 epps.pos -= m_qrot*Vec3(0,0,m_dh);
2109 m_pWorld->OnEvent(m_flags,&epps);
2110 }
2111 /*if (m_pWorld->m_iLastLogPump > m_timeLogged)
2112 m_posLogged = pos0;
2113 m_timeLogged = m_pWorld->m_iLastLogPump;*/
2114 }
2115
2116 return 1;
2117 }
2118
2119
2120
2121 float CLivingEntity::CalcEnergy(float time_interval)
2122 {
2123 float E=m_mass*sqr(m_vel);
2124 if (m_pBody)
2125 E = max(E, m_mass*m_pBody->v.len2());
2126 for(int i=0;i<m_nContacts;i++) // account for extra energy we are going to add by enforcing penertation unprojections
2127 E += sqr(min(3.0f,(m_pContacts[i].penetration*10.0f)))*m_pContacts[i].pent->GetRigidBody(m_pContacts[i].ipart)->M;
2128 return E;
2129 }
2130
2131 int CLivingEntity::RegisterContacts(float time_interval,int nMaxPlaneContacts)
2132 {
2133 int i,j;
2134 Vec3 pt[2],axis=m_qrot*Vec3(0,0,1);
2135 float h;
2136 entity_contact *pcontact;
2137
2138 if (m_iSimClass!=7) for(i=0;i<m_nContacts;i++) {
2139 h = (m_pContacts[i].pt-m_pContacts[i].center)*axis;
2140 pt[0] = pt[1] = m_pContacts[i].pt;
2141 if (fabs_tpl(m_pContacts[i].n*axis)>0.7f && fabs_tpl(h)>m_size.z*0.9f) { // contact with cap
2142 pt[1] += (m_pContacts[i].center-pt[0])*2; pt[1].z = pt[0].z;
2143 } else { // contact with side
2144 pt[0] -= axis*(h-m_size.z); pt[1] -= axis*(h+m_size.z);
2145 }
2146
2147 for(j=0;j<2;j++) {
2148 if (!(pcontact=(entity_contact*)AllocSolverTmpBuf(sizeof(entity_contact))))
2149 return 0;
2150 pcontact->flags = 0;
2151 pcontact->pent[0] = m_pContacts[i].pent;
2152 pcontact->ipart[0] = m_pContacts[i].ipart;
2153 pcontact->pbody[0] = m_pContacts[i].pent->GetRigidBody(m_pContacts[i].ipart);
2154 pcontact->pent[1] = this;
2155 pcontact->ipart[1] = 0;
2156 pcontact->pbody[1] = GetRigidBody();
2157 pcontact->friction = 0.5f;
2158 pcontact->vreq = m_pContacts[i].n*min(3.0f,(m_pContacts[i].penetration*10.0f));
2159 pcontact->pt[0] = pcontact->pt[1] = pt[j];
2160 pcontact->n = m_pContacts[i].n;
2161 //pcontact->K.SetZero();
2162 //m_pContacts[i].pent->GetContactMatrix(m_pContacts[i].pt, m_pContacts[i].ipart, pcontact->K);
2163 ::RegisterContact(pcontact);
2164 m_pContacts[i].pSolverContact[j] = pcontact;
2165 }
2166 DisablePreCG();
2167 }
2168
2169 if (m_pBody && (!m_bFlying || m_bSwimming) && (pcontact=(entity_contact*)AllocSolverTmpBuf(sizeof(entity_contact)))) {
2170 pcontact->pent[0] = this;
2171 pcontact->ipart[0] = -1;
2172 pcontact->pbody[0] = m_pBody;
2173 if (m_pLastGroundCollider) {
2174 pcontact->pent[1] = m_pLastGroundCollider;
2175 pcontact->ipart[1] = m_iLastGroundColliderPart;
2176 } else {
2177 pcontact->pent[1] = &g_StaticPhysicalEntity;
2178 pcontact->ipart[1] = -1;
2179 }
2180 pcontact->pbody[1] = pcontact->pent[1]->GetRigidBody(pcontact->ipart[1]);
2181 pcontact->friction = m_velRequested.len2() || m_slopeClimb<=0 ? 0.0f : (1-sqr(m_slopeClimb))/m_slopeClimb;
2182 pcontact->pt[0]=pcontact->pt[1] = m_pos;
2183 pcontact->n = m_bFlying ? m_qrot*Vec3(0,0,1):m_nslope;
2184 //pcontact->K.SetZero();
2185 //pcontact->K(0,0)=pcontact->K(1,1)=pcontact->K(2,2) = m_massinv;
2186 //pcontact->pent[1]->GetContactMatrix(pcontact->pt[1], pcontact->ipart[1], pcontact->K);
2187 ::RegisterContact(pcontact);
2188 DisablePreCG();
2189 }
2190
2191 return 1;
2192 }
2193
2194 int CLivingEntity::Update(float time_interval, float damping)
2195 {
2196 int i,j;
2197 for(i=0;i<m_nContacts;i++) if (m_pContacts[i].pSolverContact[0])
2198 RegisterContact(m_pos,m_pContacts[i].pt,m_pContacts[i].n, m_pContacts[i].pent,m_pContacts[i].ipart, m_surface_idx,
2199 m_pContacts[i].pSolverContact[0]->Pspare+m_pContacts[i].pSolverContact[1]->Pspare);
2200 for(i=j=0;i<m_nColliders;i++) if (!m_pColliders[i]->HasConstraintContactsWith(this)) {
2201 m_pColliders[i]->RemoveColliderMono(this);
2202 m_pColliders[i]->Release();
2203 } else
2204 m_pColliders[j++] = m_pColliders[i];
2205 m_nColliders = j;
2206 m_nContacts = 0;
2207 if (m_pBody) {
2208 const Vec3 v = m_pBody->v * damping;
2209 const Vec3 w = m_pBody->w * damping;
2210 m_pBody->v = v;
2211 m_pBody->w = w;
2212 m_vel = v;
2213 m_bJumpRequested |= isneg(-v.z);
2214
2215 if ((m_pBody->Ibody*w).len2()>0) {
2216 m_qrot *= Quat::CreateRotationAA(w.len()*time_interval, Vec3(0,0,sgnnz((w*m_qrot).z)));
2217 m_qrot.Normalize();
2218 m_pBody->w.zero(); m_pBody->L.zero();
2219 }
2220 }
2221 return 1;
2222 }
2223
2224
2225 RigidBody *CLivingEntity::GetRigidBody(int ipart,int bWillModify)
2226 {
2227 if (!bWillModify)
2228 return CPhysicalEntity::GetRigidBody(ipart);
2229 else {
2230 if (!m_pBody) {
2231 m_pBody = new RigidBody;
2232 m_pBody->Create(m_pos,Vec3(1),m_qrot,m_size.GetVolume()*8+gf_PI*(4.0f/3)*cube(m_size.x)*m_bUseCapsule,m_mass,m_qrot,m_pos);
2233 }
2234 Vec3 axis = m_qrot*Vec3(0,0,1);
2235 m_pBody->pos=m_pos+axis*m_hCyl; m_pBody->q=m_qrot;
2236 m_pBody->P=(m_pBody->v=m_vel)*(m_pBody->M=m_mass);
2237 m_pBody->Minv=m_massinv;
2238 m_pBody->Ibody_inv.zero();
2239 m_pBody->Iinv.SetZero();
2240 m_pBody->Ibody.zero();
2241 //m_pBody->Ibody_inv.z = m_massinv/(g_PI*m_size.z*sqr(sqr(m_size.x)));
2242 //dotproduct_matrix(axis,axis*m_pBody->Ibody_inv.z, m_pBody->Iinv);
2243 //m_pBody->w.zero(); m_pBody->L.zero();
2244 if (m_kInertia==0)
2245 m_timeForceInertia = m_timeImpulseRecover;
2246 return m_pBody;
2247 }
2248 }
2249
2250
2251 void CLivingEntity::OnContactResolved(entity_contact *pContact, int iop, int iGroupId)
2252 {
2253 if (!(pContact->flags & contact_angular)) {
2254 Vec3 n;
2255 if (pContact->flags & contact_rope_stretchy)
2256 return;
2257 if (!(pContact->flags & contact_rope) || iop==0)
2258 n = pContact->n;
2259 else
2260 n = iop==1 ? pContact->vreq : ((rope_solver_vtx*)pContact->pBounceCount)[iop-2].v.normalized();
2261 m_velRequested -= n*min(0.0f,m_velRequested*n);
2262 m_vel -= n*min(0.0f,m_vel*n);
2263 }
2264 }
2265
2266
2267 void CLivingEntity::DrawHelperInformation(IPhysRenderer *pRenderer, int flags)
2268 {
2269 if (m_bActive || m_nParts>0 && m_parts[0].flags & collider_flags) {
2270 CPhysicalEntity::DrawHelperInformation(pRenderer, flags);
2271
2272 geom_world_data gwd;
2273 gwd.R = Matrix33(m_qrot);
2274 gwd.offset = m_pos + gwd.R*m_parts[0].pos;
2275 if (m_pHeadGeom) {
2276 gwd.offset += gwd.R*Vec3(0,0,m_hHead-m_hCyl-m_dh);
2277 m_pHeadGeom->DrawWireframe(pRenderer,&gwd,0,m_iSimClass);
2278 }
2279 }
2280 }
2281
2282
2283 void CLivingEntity::GetMemoryStatistics(ICrySizer *pSizer) const
2284 {
2285 if (GetType()==PE_LIVING)
2286 pSizer->AddObject(this, sizeof(CLivingEntity));
2287 CPhysicalEntity::GetMemoryStatistics(pSizer);
2288 }
2289
2290 void CLivingEntity::Step_HandleFlying(Vec3 &vel, const Vec3& velGround, int bWasFlying, const Vec3& heightAdj, const float kInertia, const float time_interval)
2291 {
2292 Vec3 last_force, gravity, gravityAdjusted;
2293 if (!bWasFlying)
2294 vel += velGround;
2295 if (!m_pWorld->m_vars.bFlyMode) {
2296 gravity = m_gravity * time_interval;
2297 if ((gravity*heightAdj)<0.f) { // Remove any slope adjustment from the gravity accumulation
2298 gravityAdjusted = gravity + heightAdj/time_interval;
2299 if (gravityAdjusted*gravity>0.f)
2300 vel += gravityAdjusted;
2301 } else
2302 vel += gravity;
2303 if (m_bSwimming)
2304 vel += (m_velRequested-vel)*kInertia*time_interval;
2305 /*if ((vel+last_force*time_interval)*vel<0)
2306 vel.zero();
2307 else*/
2308 last_force = -vel*m_kAirResistance*time_interval;
2309 if (last_force.len2()<vel.len2()*4.0f)
2310 vel += last_force;
2311 }
2312 if (vel.len2()<1E-10f) vel.zero();
2313 ReleaseGroundCollider();
2314 }
2315
2316 void CLivingEntity::Step_HandleWasFlying(Vec3& vel, int& bFlying, const Vec3& axis, const int bGroundContact)
2317 {
2318 if (!m_bSwimming) {
2319 if (vel*axis<-4.0f && !m_bStateReading && (m_nodSpeed > 0.0f)) {
2320 m_dhSpeed = vel*axis;
2321 m_dhAcc = max(sqr(m_dhSpeed)/(m_size.z*2), m_nodSpeed);
2322 }
2323 vel -= m_velGround;
2324 if (m_nslope*axis<m_slopeFall && (vel*axis<-4) && !m_bStuck){
2325 vel -= m_nslope*(vel*m_nslope)*1.0f;
2326 if (vel*axis>0) vel -= axis*(axis*vel);
2327 bFlying = 1;
2328 } else if (m_nslope*axis<m_slopeFall || (!bGroundContact || m_velRequested.len2()>0) && (m_nslope*axis>m_slopeClimb || vel*axis<0))
2329 vel -= m_nslope*(vel*m_nslope);
2330 else
2331 vel.zero();
2332 }
2333 }
CRYENGINE is a powerful real-time game development platform created by Crytek.