1 // Copyright 2001-2018 Crytek GmbH / Crytek Group. All rights reserved.
7 #include "singleboxtree.h"
8 #include "cylindergeom.h"
9 #include "capsulegeom.h"
10 #include "spheregeom.h"
13 #include "rigidbody.h"
14 #include "physicalplaceholder.h"
15 #include "physicalentity.h"
17 #include "physicalworld.h"
18 #include "livingentity.h"
19 #include "rigidentity.h"
22 #define collider_flags (m_parts[0].flagsCollider)
23 #define GROUND_EPS_SIZE max(0.01f*m_size.z, 0.004f)
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
)));
31 inline Vec3
DecodeVec6b(const Vec3_tpl
<short> &vec
) {
32 return Vec3(vec
.x
,vec
.y
,vec
.z
)*(100.0f
/32767);
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
));
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
;
56 CLivingEntity::CLivingEntity(CPhysicalWorld
*pWorld
, IGeneralMemoryHeap
* pHeap
)
57 : CPhysicalEntity(pWorld
, pHeap
)
59 , m_velRequested(ZERO
)
60 , m_gravity(0, 0, -9.81f
)
61 , m_nslope(0.0f
, 0.0f
, 1.0f
)
64 , m_kInertiaAccel(0.0f
)
66 , m_kAirResistance(0.0f
)
70 , m_size(0.4f
, 0.4f
, 0.6f
)
78 , m_massinv(2.0f
/ 80)
80 , m_lastGroundSurfaceIdx(-1)
81 , m_lastGroundSurfaceIdxAux(-1)
82 , m_lastGroundPrim(-1)
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)
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
)
101 , m_iTimeLastSend(-1)
102 , m_collTypes(ent_terrain
| ent_static
| ent_sleeping_rigid
| ent_rigid
| ent_living
)
103 , m_pLivingEntToIgnore(nullptr)
105 , m_bJumpRequested(0)
108 , m_bIgnoreCommands(0)
111 , m_bActiveEnvironment(0)
113 , m_bReleaseGroundColliderWhenNotActive(1)
114 , m_bHadCollisions(0)
116 , m_pLastGroundCollider(nullptr)
117 , m_iLastGroundColliderPart(0)
118 , m_posLastGroundColl(ZERO
)
123 , m_deltaQRot(IDENTITY
)
124 , m_timeSmooth(0.16f
)
125 , m_timeRotChanged(-1.0f
)
126 , m_pContacts(nullptr)
128 , m_nContactsAlloc(0)
135 m_gravity
= pWorld
->m_vars
.gravity
;
140 cyl
.axis
.Set(0,0,1); cyl
.center
.zero();
141 cyl
.hh
= m_size
.z
; cyl
.r
= m_size
.x
;
145 ((CCapsuleGeom
*)(m_pCylinderGeom
= new CCapsuleGeom
))->CreateCapsule((capsule
*)&cyl
);
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
;
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;
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
;
177 CLivingEntity::~CLivingEntity()
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;
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
;
193 void CLivingEntity::ReleaseGroundCollider()
195 if (m_pLastGroundCollider
) {
196 m_pLastGroundCollider
->Release();
197 m_pLastGroundCollider
= 0;
201 void CLivingEntity::SetGroundCollider(CPhysicalEntity
*pCollider
, int bAcceptStatic
)
203 ReleaseGroundCollider();
204 if (pCollider
&& pCollider
->m_iSimClass
+bAcceptStatic
>0) {
206 m_pLastGroundCollider
= pCollider
;
210 int CLivingEntity::Awake(int bAwake
,int iSource
)
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();
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
226 geom_world_data gwd
[3];
227 intersection_params ip
;
228 CPhysicalEntity
**pentlist
;
229 geom_contact
*pcontacts
;
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
);
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
;
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;
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
)) {
257 if (dirUnproj
.len2()==0) dirUnproj
= pcontacts
[0].dir
;
258 t
= pcontacts
[0].t
; // lock should be released after reading t
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
))
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
))
281 int CLivingEntity::SetParams(pe_params
*_params
, int bThreadSafe
)
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
);
291 { ReadLock
lock(m_lockUpdate
);
292 hCyl
=m_hCyl
; size
=m_size
; hPivot
=m_hPivot
;
293 pos
=m_pos
; qrot
=m_qrot
;
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
;
299 if ((heightCollider
!=hCyl
|| sizeCollider
.x
!=size
.x
|| sizeCollider
.z
!=size
.z
) && m_pWorld
&& m_bActive
)
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)
313 quaternionf prevq
= m_qrot
;
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;
322 for(int i
=0;i
<3;i
++) if (fabs_tpl(m_qrot
.v
[i
])<1e-10f
)
324 if (fabs_tpl(m_qrot
.w
)<1e-10f
)
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
;
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);
342 /*if (m_qrot.s!=prevq.s || m_qrot.v!=prevq.v) {
344 m_qrot.get_Euler_angles_xyz(angles);
345 m_qrot = quaternionf(angles.z,Vec3(0,0,1));
347 //m_BBox[0].z = min(m_BBox[0].z,m_pos.z-m_hPivot);
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
;
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
)
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)
371 { WriteLock
lock(m_lockUpdate
);
372 m_pos
+= params
->dirUnproj
*unproj
;
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
;
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
);
385 if (m_bUseCapsule
= params
->bUseCapsule
)
386 ((CCapsuleGeom
*)m_pCylinderGeom
)->CreateCapsule((capsule
*)&newdim
);
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
);
393 sph
.center
.Set(0,0,-m_size
.z
);
396 if (!is_unused(params
->groundContactEps
)) m_groundContactEps
= max(params
->groundContactEps
, GROUND_EPS_SIZE
);
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);
403 m_hEye
= params
->heightEye
;
404 m_timeSinceStanceChange
= 0;
407 if (!is_unused(params
->heightHead
)) m_hHead
= params
->heightHead
;
408 if (!is_unused(params
->headRadius
)) {
410 sph
.center
.zero(); sph
.r
= params
->headRadius
;
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;
420 m_pWorld
->RepositionEntity(this,1|8);
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)
451 m_bActive
= params
->bActive
;
460 int CLivingEntity::GetParams(pe_params
*_params
) const
463 if (res
= CPhysicalEntity::GetParams(_params
))
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
;
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
;
509 int CLivingEntity::AddGeometry(phys_geometry
*pgeom
, pe_geomparams
* params
,int id
, int bThreadSafe
)
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
;
518 void CLivingEntity::RemoveGeometry(int id
, int bThreadSafe
)
520 CPhysicalEntity::RemoveGeometry(id
);
522 m_parts
[0].flags
|= geom_colltype_ray
;
526 int CLivingEntity::GetStatus(pe_status
*_status
) const
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)
538 m_pos = m_posLogged;*/
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
;
544 status
->flags
= flags
;
548 if (CPhysicalEntity::GetStatus(_status
))
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
);
558 if (m_pLastGroundCollider
)
559 status
->vel
+= m_velGround
;
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
;
577 if (_status
->type
==pe_status_dynamics::type_id
) {
578 pe_status_dynamics
*status
= (pe_status_dynamics
*)_status
;
579 ReadLock
lock(m_lockLiving
);
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
);
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;
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;
615 if (_status
->type
==pe_status_collisions::type_id
) {
616 // pe_status_collisions *status = (pe_status_collisions*)_status;
617 return m_bHadCollisions
--;
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
;
632 return ((status
->unproj
= UnprojectionNeeded(pos
,qrot
,hCollider
,m_hPivot
,size
,bCapsule
,status
->dirUnproj
))==0);
639 int CLivingEntity::Action(pe_action
* _action
, int bThreadSafe
)
641 ChangeRequest
<pe_action
> req(this,m_pWorld
,_action
,bThreadSafe
);
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
);
651 WriteLockCond
lock(m_lockLiving
,bThreadSafe
^1);
652 if (!is_unused(action
->dir
)) {
653 //m_velRequested.zero();
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)
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());
665 if (action
->iJump
==2) {
666 m_vel
+= action
->dir
; bForceHistory
= true; m_forceFly
= false;
667 if (action
->dir
.z
>1.0f
) {
669 m_bJumpRequested
= 1;
672 m_vel
= action
->dir
; bForceHistory
= true; m_forceFly
= false;
674 m_bJumpRequested
= 1;
677 if (m_flags
& lef_snap_velocities
) {
678 m_velRequested
= DecodeVec4b(EncodeVec4b(m_velRequested
));
679 m_vel
= DecodeVec6b(EncodeVec6b(m_vel
));
681 m_dtRequested
= action
->dt
;
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
);
692 WriteLock
lock(m_lockLiving
);
693 Vec3 impulse
= action
->impulse
;
694 if (action
->iSource
==2) // explosion
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;
707 am
.dir
= impulse
*m_massinv
;
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);
716 m_timeForceInertia
= m_timeImpulseRecover
;
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;
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
);
732 if (!is_unused(action
->v
)) {
733 m_velRequested
= m_vel
= action
->v
;
734 m_bJumpRequested
= 1;
740 return CPhysicalEntity::Action(_action
,1);
744 int CLivingEntity::GetStateSnapshot(CStream
&stm
,float time_back
,int flags
)
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
;
764 if (m_flags
& lef_snap_velocities
) {
765 if (m_vel
.len2()>0) {
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) {
772 vec4b vr
= EncodeVec4b(m_velRequested
);
773 stm
.Write(vr
.yaw
); stm
.Write(vr
.pitch
); stm
.Write(vr
.len
);
774 } else stm
.Write(false);
776 if (m_vel
.len2()>0) {
779 } else stm
.Write(false);
780 if (m_velRequested
.len2()>0) {
782 stm
.Write(m_velRequested
);
783 } else stm
.Write(false);
785 if (m_timeFlying
>0) {
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));
793 /*if (m_pLastGroundCollider) {
795 WritePacked(stm, m_pWorld->GetPhysicalEntityId(m_pLastGroundCollider)+1);
796 WritePacked(stm, m_iLastGroundColliderPart);
797 stm.Write((Vec3&)m_posLastGroundColl);
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
;
811 void SLivingEntityNetSerialize::Serialize( TSerialize ser
)
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
);
827 idEntGroundCollider
= -1;
828 posOnGroundCollider
.zero();
832 int CLivingEntity::GetStateSnapshot(TSerialize ser
, float time_back
, int flags
)
834 WriteLock
lock0(m_lockUpdate
),lock1(m_lockLiving
);
836 SLivingEntityNetSerialize helper
= {
842 m_dh
, m_dhSpeed
, m_stablehTime
,
843 m_pWorld
->GetPhysicalEntityId(m_pLastGroundCollider
), m_iLastGroundColliderPart
, m_posLastGroundColl
845 helper
.Serialize( ser
);
851 int CLivingEntity::SetStateFromSnapshot(CStream
&stm
, int flags
)
853 bool bnz
,bCompressed
;
856 stm
.ReadNumberInBits(ver
,4);
857 if (ver
!=SNAPSHOT_VERSION
)
860 WriteLock
lock0(m_lockUpdate
),lock1(m_lockLiving
);
861 if (flags
& ssf_no_update
) {
863 if (m_pWorld
->m_vars
.bMultiplayer
)
864 ReadCompressedPos(stm
,dummy
,bCompressed
);
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);
874 m_posLocal
= m_pos
+ m_deltaPos
*(m_timeSmooth
*(1/0.3f
));
876 Vec3 pos0
=m_pos
/*,vel0=m_vel,velRequested0=m_velRequested*/;
877 // int bFlying0=m_bFlying;
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
))
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
);
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();
897 stm
.Read(bnz
); if (bnz
)
900 stm
.Read(bnz
); if (bnz
)
901 stm
.Read(m_velRequested
);
902 else m_velRequested
.zero();
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
};
910 m_bFlying
= bnz
? 1:0;
911 ReleaseGroundCollider();
912 m_bJumpRequested
= 0;
916 SetGroundCollider((CPhysicalEntity*)m_pWorld->GetPhysicalEntityById(ReadPackedInt(stm)-1));
917 m_iLastGroundColliderPart = ReadPackedInt(stm);
918 stm.Read((Vec3&)m_posLastGroundColl);
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
));
926 if (m_pos
.len2()>1E18
) {
935 int CLivingEntity::SetStateFromSnapshot(TSerialize ser
, int flags
)
937 SLivingEntityNetSerialize helper
;
938 helper
.Serialize( ser
);
940 if ((ser
.GetSerializationTarget()==eST_Network
) && (helper
.pos
.len2()<50.0f
))
943 if (ser
.ShouldCommitValues() && m_pWorld
)
945 pe_params_pos setpos
; setpos
.bRecalcBounds
|= 16|32;
946 pe_action_set_velocity velocity
;
948 // Vec3 debugOnlyOriginalHelperPos = helper.pos;
950 if (flags
& ssf_compensate_time_diff
)
952 float dtBack
=(m_pWorld
->m_iTimeSnapshot
[1]-m_pWorld
->m_iTimeSnapshot
[0])*m_pWorld
->m_vars
.timeGranularity
;
953 helper
.pos
+= helper
.vel
* dtBack
;
956 const float MAX_DIFFERENCE
= std::max( helper
.vel
.GetLength() * 0.1f
, 0.1f
);
960 IPersistantDebug * pPD = gEnv->pGameFramework->GetIPersistantDebug();
961 Vec3 pts[3] = {debugOnlyOriginalHelperPos, helper.pos, m_pos};
962 pPD->Begin("Snap", false);
964 for (int i=0; i<3; i++)
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 );
970 pPD->Begin("Snap2", true);
971 pPD->AddPlanarDisc( m_pos + bump*0.5f, 0, MAX_DIFFERENCE, ColorF(0,0,1,0.5), 0.3f );
975 float distance
= m_pos
.GetDistance(helper
.pos
);
976 setpos
.pos
= helper
.pos
;
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
980 WriteLock
lock1(m_lockLiving
);
982 m_velRequested
= helper
.velRequested
;
983 m_bFlying
= helper
.bFlying
;
984 m_bJumpRequested
= helper
.bJumpRequested
;
986 m_dhSpeed
= helper
.dhSpeed
;
987 m_stablehTime
= helper
.stablehTime
;
989 SetGroundCollider((CPhysicalEntity
*)m_pWorld
->GetPhysicalEntityById(helper
.idEntGroundCollider
));
990 m_iLastGroundColliderPart
= helper
.ipartGroundCollider
;
991 m_posLastGroundColl
= helper
.posOnGroundCollider
;
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
)
1001 int i
,j
,jbest
,ncont
,idbest
,idbestAux
=-1,iPrim
=-1,bHasMatSubst
=0;
1002 int j1
,iCaller
=get_iCaller_int();
1004 Vec3 pt
,axis
=m_qrot
*Vec3(0,0,1);
1005 float h
=-1E10f
,maxdim
,maxarea
,haux
=-1E10f
;
1008 R
= Matrix33(m_qrot
);
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
));
1015 R
= Matrix33(Quat(0,1,0,0));
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
;
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
]))
1032 if (j
<pent
->m_nColliders
)
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
)
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
);
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
)
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
1063 idbest
= pent
->GetMatId(pcontacts
[ncont
-1].id
[0], pparts
[j1
].ipart
);
1064 iPrim
= pcontacts
[ncont
-1].iPrim
[0];
1071 if (bUpdateGroundCollider
) {
1072 m_lastGroundSurfaceIdx
=m_lastGroundSurfaceIdxAux
= -1;
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
;
1088 if (pPrevCollider
!=pentbest
)
1089 AddLegsImpulse(m_gravity
*time_interval
+m_vel
,nslope
,true);
1091 AddLegsImpulse(m_gravity
*time_interval
,nslope
,false);
1093 pe_status_dynamics sd
;
1095 pentbest
->GetStatus(&sd
);
1096 m_velGround
= sd
.v
+(sd
.w
^pt
-sd
.centerOfMass
);
1098 else if (m_pLastGroundCollider
)
1099 ReleaseGroundCollider();
1105 void CLivingEntity::AddLegsImpulse(const Vec3
&vel
, const Vec3
&nslope
, bool bInstantChange
)
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
;
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
))
1121 ai
.point
= ssca
.ptTest
;
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);
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
)
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
);
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))
1152 epc
.n
= v
.normalized();
1153 imp
= (epc
.n
*(v
-m_vel
-m_velGround
))*(m_mass
*(m_mass
*pbody
->Minv
+1.0f
));
1157 epc
.vloc
[0] = m_vel
+m_velGround
;
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))
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
;
1176 epc
.iPrim
[1] = iPrim
;
1177 m_pWorld
->OnEvent(m_flags
,&epc
);
1178 m_bHadCollisions
= 1;
1182 void CLivingEntity::RegisterUnprojContact(const le_contact
&unproj
)
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
));
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);
1199 float CLivingEntity::GetMaxTimeStep(float time_interval
)
1201 if (m_timeStepPerformed
> m_timeStepFull
-0.001f
)
1202 return time_interval
;
1203 return min_safe(m_timeStepFull
-m_timeStepPerformed
,time_interval
);
1206 Vec3
CLivingEntity::SyncWithGroundCollider(float time_interval
)
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
;
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
);
1224 pe_status_dynamics sd
;
1226 m_pLastGroundCollider
->GetStatus(&sd
);
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
;
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
;
1240 pe_action_impulse ai
;
1245 m_pLastGroundCollider
->Action(&ai
);
1249 if (m_velGround
.len2()>sqr(m_maxVelGround
))
1250 m_velGround
.normalize() *= m_maxVelGround
;
1252 if (m_pWorld
->m_bWorldStep
==2)
1258 void CLivingEntity::ComputeBBox(Vec3
*BBox
, int flags
)
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
);
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
);
1273 void CLivingEntity::UpdatePosition(const Vec3
&pos
, const Vec3
*BBox
, int bGridLocked
)
1275 WriteLockCond
lock(m_lockUpdate
,m_bStateReading
^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];
1283 for(int i
=0;i
<m_nParts
;i
++) m_parts
[i
].pNewCoords
= (coord_block_BBox
*)&m_parts
[i
].pos
;
1284 ComputeBBox(m_BBox
);
1286 m_pWorld
->UnlockGrid(this,-bGridLocked
);
1289 void CLivingEntity::StartStep(float time_interval
)
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
;
1298 m_nContacts
= 0; m_bMoved
= 0;
1302 int CLivingEntity::Step(float time_interval
)
1304 if (time_interval
<=0)
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
);
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;
1332 IF (nNoResponseAllocLE
==0, 0) pNoResponseContactLE
=new le_tmp_contact
[nNoResponseAllocLE
=16];
1334 if (m_timeForceInertia
>0.0001f
)
1336 else if (m_kInertiaAccel
&& m_velRequested
.len2()>0.1f
)
1337 kInertia
= m_kInertiaAccel
;
1339 kInertia
= m_kInertia
;
1341 pos
= m_pWorld
->m_bWorldStep
==2 ? m_pos
: SyncWithGroundCollider(time_interval
);
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
;
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))
1356 CRY_PROFILE_FUNCTION(PROFILE_PHYSICS
);
1357 PHYS_ENTITY_PROFILER
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
);
1365 if (bFlying
&& !m_pWorld
->m_vars
.bFlyMode
&& m_kAirControl
>0) {
1367 Vec3 velDelta
= m_velRequested
* (m_kInertia
*time_interval
*m_kAirControl
);
1368 Vec3 velDiff
= m_velRequested
- vel
;
1370 const float kAirControlSelect
= m_kAirControl
-1.0f
;
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
);
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
));
1380 velDelta
.y
= (float)__fsel(kAirControlSelect
, velDiff
.y
, velDelta
.y
);
1381 velDelta
.x
= (float)__fsel(kAirControlSelect
, velDiff
.x
, velDelta
.x
);
1384 } else if (m_gravity
.len2()>0)
1385 vel
= m_gravity
*(vel
*m_gravity
-m_velRequested
*m_gravity
)/m_gravity
.len2()+m_velRequested
;
1387 vel
= m_velRequested
;
1390 //filippo:m_forceFly is to let the game set a velocity no matter what is the status of the entity.
1392 vel
= m_velRequested
;
1393 else if (bFlying
&& !m_bSwimming
&& !m_pWorld
->m_vars
.bFlyMode
)
1394 move
+= m_gravity
*sqr(time_interval
)*0.5f
;
1396 if (vel
.len2() > sqr(m_pWorld
->m_vars
.maxVelPlayers
))
1397 vel
.normalize() *= m_pWorld
->m_vars
.maxVelPlayers
;
1399 move
+= vel
*time_interval
;
1403 axis
= m_qrot
*Vec3(0,0,1);
1404 if (_isnan(move
.len2())) //necessary? Is there any way that this can occur? - Rich S
1406 --m_bSquashed
; m_bSquashed
-=m_bSquashed
>>31;
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();
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));
1423 nents
= m_pWorld
->GetEntitiesAround(BBoxOuter0
,BBoxOuter1
,
1424 pentlist
, m_collTypes
|ent_independent
|ent_triggers
|ent_sort_by_mass
, this, 0,iCaller
);
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();
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];
1433 if (IgnoreCollision(m_collisionClass
, pentlist
[i
]->m_collisionClass
))
1435 if (pentlist
[i
]->m_iSimClass
==2) {
1436 if (pentlist
[i
]->m_flags
& ref_small_and_fast
)
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
));
1444 if (!bFastPhys
&& !AABB_overlap(pentlist
[i
]->m_BBox
,BBoxInner
) && size
.len2()>0)
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))
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();
1461 pentlist
[j
++] = pentlist
[i
];
1466 bStaticUnproj
= bDynUnproj
= 0;
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
];
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;
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
);
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();
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
;
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
;
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
;
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
;
1539 gwd
[1].v
.zero(); gwd
[1].w
.zero();
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
;
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
))
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);
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
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
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
));
1587 ncontactSum
= pcontacts
[ncont
].n
;
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
]);
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
)));
1601 } else if (!(bVeryHeavyFlags
& pef_cannot_squash_players
)) {
1602 m_bSquashed
= min(5, m_bSquashed
+5*isneg(ncontactSum
*pcontacts
[ncont
].n
+0.99f
));
1604 ncontactSum
= pcontacts
[ncont
].n
;
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
;
1612 if (pcontacts
[ncont
].t
>m_size
.x
)
1613 ip
.ptOutsidePivot
[0].Set(1E11f
,1E11f
,1E11f
);
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;
1625 RigidBody
body(false), *pbody
=pent
->GetRigidBodyTrans(&body
, j
, this, 0,true);
1626 pbody
->GetContactMatrix(pcontacts
[ncont
].center
-pbody
->pos
, K
);
1628 else if (!(uFlags
& pef_pushable_by_players
))
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();
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
;
1647 vel.z = max(0.0f, vel.z); vel.z += ai.impulse.len()*fMassInv*0.1f;
1649 bFlying
= 1; m_timeFlying
= 0;
1651 m_timeForceInertia
= m_timeImpulseRecover
;
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;
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]);
1671 if (bStaticUnproj
&& bDynUnproj
&& m_bSquashed
) {
1672 pos
= pos0
; // disable unprojection if we are being squashed
1673 bStaticUnproj
= bDynUnproj
= 0;
1675 else if (bStaticUnproj
+bDynUnproj
>0) {
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
))
1686 if (h
>hcur
&& m_nslope
*axis
<m_slopeSlide
&& m_nslope
*nslope
<m_slopeSlide
&&
1687 fAxisDotSlope
<m_slopeSlide
&& m_velRequested
.len2()==0)
1689 newpos
= pos
; vel
.zero();
1691 newpos
+= (heightAdj
= axis
*(h
+m_hPivot
-newpos
*axis
));
1692 move
= newpos
-pos
; movelen
= move
.len();
1695 if (m_bJumpRequested
)
1696 AddLegsImpulse(-vel
,m_nslope
,true);
1697 m_bJumpRequested
= 0;
1698 m_bStuck
= 0; ncontactSum
.zero();
1700 if (movelen
>m_size
.x
*1E-4f
&& m_parts
[0].flagsCollider
!=0) {
1701 ip
.bSweepTest
= true;
1702 gwd
[0].v
= move
/movelen
;
1704 float move0
= movelen
; movesum
= 0;
1705 pe_player_dimensions pd
;
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
)
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
)))
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
))
1732 gwd
[1].R
= Matrix33(pPrecompParts
[j1
].partrot
);
1733 gwd
[1].offset
= pPrecompParts
[j1
].partoff
;
1734 gwd
[1].scale
= pPrecompParts
[j1
].partscale
;
1737 if((ncont
= m_pCylinderGeom
->Intersect(pPrecompParts
[j1
].pgeom
, gwd
,gwd
+1, &ip
, pcontacts
)))
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
)) {
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);
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
)))
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
;
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);
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);
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;
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
))
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;
1801 IF (pNRC
>=(pNoResponseContactLE
+nNoResponseAllocLE
-1), 0) {
1802 delete [] pNoResponseContactLE
; pNoResponseContactLE
=new le_tmp_contact
[nNoResponseAllocLE
+16];
1803 pNRC
=pNoResponseContactLE
+nNoResponseAllocLE
-1; nNoResponseAllocLE
+=16;
1805 ++pNRC
; pNRC
->pent
=NULL
; pNRC
->tmin
=ip
.time_interval
*2;
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
);
1814 if ((ncontact*axis)*(1-m_bSwimming)>g_cosSlide)
1815 ncontact = axis*g_cosSlide + (pos-ptcontact-axis*(axis*(pos-ptcontact))).normalized()*g_sinSlide;
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;
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
;
1831 RigidBody
body(false), *pbody
=pentmin
->GetRigidBodyTrans(&body
, jmin
, this, 0,true);
1832 pbody
->GetContactMatrix(ptcontact
-pbody
->pos
, K
);
1834 if (!bPushOther
|| /*pentmin->m_iSimClass-3 | */m_flags
& pef_pushable_by_players
)
1835 K
+= Matrix33(Diag33(m_massinv
));
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
))
1849 RigidBody
body(false), *pbody
= pentmin
->GetRigidBodyTrans(&body
,jmin
,this);
1850 vrel
-= ncontact
*(pbody
->v
+(pbody
->w
^ptcontact
-pbody
->pos
));
1852 vrel
= min(0.0f
, vrel
);
1853 float imp
= -vrel
*1.01f
/max(1e-6f
,ncontact
*K
*ncontact
);
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
);
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
;
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
;
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
)
1873 pentmin
->Action(&ai
);
1874 if (pentmin
->m_iSimClass
<3)
1875 m_timeSinceImpulseContact
= 0;
1878 movelen
-= tmin
; movesum
+= tmin
;
1879 for(i
=0;i
<iter
&& ncontactHist
[i
]*ncontact
<0.95f
;i
++);
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
);
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
);
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
);
1894 pos
+= gwd
[0].v
*movelen
; movesum
+= movelen
; /* exit do-loop */ iter
=1000;
1896 for (le_tmp_contact
* c
=pNoResponseContactLE
; c
<=pNRC
&& c
->pent
; c
++)
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);
1901 const float fContactSumLenSq
= ncontactSum
.len2();
1902 if(movesum
<move0
*0.001f
&& (sqr_signed(ncontactSum
.z
)>sqr(0.4f
)*fContactSumLenSq
|| fContactSumLenSq
<0.6f
))
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
;
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
;
1930 if (!m_pLastGroundCollider
)// || m_pLastGroundCollider->GetMassInv()>m_massinv)
1933 velGround
= m_velGround
;
1934 m_hLatest
= h
= ShootRayDown(pPrecompEnts
, nents
, pPrecompParts
, pos
,nslope
, time_interval
,false,true);
1935 if (nslope
*axis
<0.087f
)
1938 WriteLockCond
lock(m_lockLiving
,m_bStateReading
^1);
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();
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();
1953 bFlying
= m_pWorld
->m_vars
.bFlyMode
|| m_gravity
*axis
>0 || m_bSwimming
|| ((bGroundContact
|m_bStuck
)^1);
1954 m_bActiveEnvironment
= m_bStuck
;
1957 Step_HandleFlying(vel
, velGround
, bWasFlying
, heightAdj
, kInertia
, time_interval
);
1960 Step_HandleWasFlying(vel
, bFlying
, axis
, bGroundContact
);
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
) {
1969 last_force
+= g
-m_nslope
*(g
*m_nslope
);
1972 const Vec3 velIncLastForce
= vel
+ (last_force
*time_interval
);
1973 if (velIncLastForce
*vel
<0 && velIncLastForce
*m_velRequested
<=0)
1976 vel
= velIncLastForce
;
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
);
1985 if (axisSlope
<m_slopeFall
&& !m_bStuck
) {
1986 bFlying
=1; vel
+= m_nslope
-axis
*(axisSlope
);
1988 if (m_velRequested
.len2()==0 && vel
.len2()<0.001f
|| vel
.len2()<0.0001f
)
1995 if (m_flags
& lef_snap_velocities
)
1996 vel
= DecodeVec6b(EncodeVec6b(vel
));
1998 if (!m_bStateReading
) {
2000 if (!bFlying
&& (dh
=(pos
-pos0
)*axis
)>m_size
.z
*0.01f
) {
2001 m_dhSpeed
= max(m_dhSpeed
, dh
/m_stablehTime
);
2006 m_stablehTime
= min(m_stablehTime
+time_interval
,0.5f
);
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;
2012 m_dh
-= m_dhSpeed
*time_interval
;
2016 ip
.bSweepTest
= true;
2017 gwd
[0].offset
= pos
+ gwd
[0].R
*m_parts
[0].pos
;
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
);
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
;
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
);
2042 m_pNewCoords
= (coord_block
*)&m_pos
;
2043 UpdatePosition(pos
,BBoxInner
, locked
);
2045 } else if (!m_bActive
) {
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);
2053 if (m_bReleaseGroundColliderWhenNotActive
!=0)
2054 ReleaseGroundCollider();
2057 { WriteLockCond
lock(m_lockLiving
,m_bStateReading
^1);
2058 //if (m_pWorld->m_vars.bMultiplayer)
2059 // m_pos = CompressPos(m_pos);
2061 m_deltaV
= (m_vel
- vel0
);
2062 m_vel
= vel
+m_vel
-vel0
;
2063 m_bFlying
= bFlying
;
2064 m_deltaQRot
= m_qrot
* !oldQRot
;
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
))
2073 if (!m_nColliders
) {
2074 delete m_pBody
; m_pBody
=0;
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);
2088 m_pBody->w = dq.v.normalized()*(acos_tpl(dq.w)*2/dt);
2093 if (!m_bStateReading
) {
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
;
2101 if (m_pWorld
->m_pWaterMan
)
2102 m_pWorld
->m_pWaterMan
->OnWaterInteraction(this);
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
);
2111 /*if (m_pWorld->m_iLastLogPump > m_timeLogged)
2113 m_timeLogged = m_pWorld->m_iLastLogPump;*/
2121 float CLivingEntity::CalcEnergy(float time_interval
)
2123 float E
=m_mass
*sqr(m_vel
);
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
;
2131 int CLivingEntity::RegisterContacts(float time_interval
,int nMaxPlaneContacts
)
2134 Vec3 pt
[2],axis
=m_qrot
*Vec3(0,0,1);
2136 entity_contact
*pcontact
;
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
);
2148 if (!(pcontact
=(entity_contact
*)AllocSolverTmpBuf(sizeof(entity_contact
))))
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
;
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
;
2177 pcontact
->pent
[1] = &g_StaticPhysicalEntity
;
2178 pcontact
->ipart
[1] = -1;
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
);
2194 int CLivingEntity::Update(float time_interval
, float damping
)
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();
2204 m_pColliders
[j
++] = m_pColliders
[i
];
2208 const Vec3 v
= m_pBody
->v
* damping
;
2209 const Vec3 w
= m_pBody
->w
* damping
;
2213 m_bJumpRequested
|= isneg(-v
.z
);
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
)));
2218 m_pBody
->w
.zero(); m_pBody
->L
.zero();
2225 RigidBody
*CLivingEntity::GetRigidBody(int ipart
,int bWillModify
)
2228 return CPhysicalEntity::GetRigidBody(ipart
);
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
);
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();
2245 m_timeForceInertia
= m_timeImpulseRecover
;
2251 void CLivingEntity::OnContactResolved(entity_contact
*pContact
, int iop
, int iGroupId
)
2253 if (!(pContact
->flags
& contact_angular
)) {
2255 if (pContact
->flags
& contact_rope_stretchy
)
2257 if (!(pContact
->flags
& contact_rope
) || iop
==0)
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
);
2267 void CLivingEntity::DrawHelperInformation(IPhysRenderer
*pRenderer
, int flags
)
2269 if (m_bActive
|| m_nParts
>0 && m_parts
[0].flags
& collider_flags
) {
2270 CPhysicalEntity::DrawHelperInformation(pRenderer
, flags
);
2272 geom_world_data gwd
;
2273 gwd
.R
= Matrix33(m_qrot
);
2274 gwd
.offset
= m_pos
+ gwd
.R
*m_parts
[0].pos
;
2276 gwd
.offset
+= gwd
.R
*Vec3(0,0,m_hHead
-m_hCyl
-m_dh
);
2277 m_pHeadGeom
->DrawWireframe(pRenderer
,&gwd
,0,m_iSimClass
);
2283 void CLivingEntity::GetMemoryStatistics(ICrySizer
*pSizer
) const
2285 if (GetType()==PE_LIVING
)
2286 pSizer
->AddObject(this, sizeof(CLivingEntity
));
2287 CPhysicalEntity::GetMemoryStatistics(pSizer
);
2290 void CLivingEntity::Step_HandleFlying(Vec3
&vel
, const Vec3
& velGround
, int bWasFlying
, const Vec3
& heightAdj
, const float kInertia
, const float time_interval
)
2292 Vec3 last_force
, gravity
, gravityAdjusted
;
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
;
2304 vel
+= (m_velRequested
-vel
)*kInertia
*time_interval
;
2305 /*if ((vel+last_force*time_interval)*vel<0)
2308 last_force
= -vel
*m_kAirResistance
*time_interval
;
2309 if (last_force
.len2()<vel
.len2()*4.0f
)
2312 if (vel
.len2()<1E-10f
) vel
.zero();
2313 ReleaseGroundCollider();
2316 void CLivingEntity::Step_HandleWasFlying(Vec3
& vel
, int& bFlying
, const Vec3
& axis
, const int bGroundContact
)
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
);
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
);
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
);