Add PhysicsEffects to Extras. The build is only tested on Windows and Android.
The Android/NEON optimized version of Physics Effects is thanks to Graham Rhodes and Anthony Hamilton, See Issue 587
This commit is contained in:
erwin.coumans
228
Extras/PhysicsEffects/src/low_level/collision/pfx_ray_cast.cpp
Normal file
228
Extras/PhysicsEffects/src/low_level/collision/pfx_ray_cast.cpp
Normal file
@@ -0,0 +1,228 @@
|
||||
/*
|
||||
Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.
|
||||
All rights reserved.
|
||||
|
||||
Physics Effects is open software; you can redistribute it and/or
|
||||
modify it under the terms of the BSD License.
|
||||
|
||||
Physics Effects is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
See the BSD License for more details.
|
||||
|
||||
A copy of the BSD License is distributed with
|
||||
Physics Effects under the filename: physics_effects_license.txt
|
||||
*/
|
||||
|
||||
#include "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"
|
||||
#include "../../../include/physics_effects/low_level/collision/pfx_ray_cast.h"
|
||||
#include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"
|
||||
#include "pfx_intersect_ray_func.h"
|
||||
#include "../../base_level/collision/pfx_intersect_common.h"
|
||||
|
||||
|
||||
namespace sce {
|
||||
namespace PhysicsEffects {
|
||||
|
||||
|
||||
void pfxRayTraverseForward(
|
||||
const PfxRayInput &ray,PfxRayOutput &out,const PfxAabb16 &rayAABB,
|
||||
PfxBroadphaseProxy *proxies,int numProxies,
|
||||
PfxRigidState *offsetRigidStates,
|
||||
PfxCollidable *offsetCollidables,
|
||||
int axis,const PfxVector3 ¢er,const PfxVector3 &half)
|
||||
{
|
||||
#ifdef SCE_PFX_USE_GEOMETRY
|
||||
PfxGeomSegment segment((PfxPoint3)ray.m_startPosition,ray.m_direction);
|
||||
#endif
|
||||
|
||||
for(int i=0;i<numProxies;i++) {
|
||||
PfxBroadphaseProxy &proxy = proxies[i];
|
||||
|
||||
// 終了条件のチェック
|
||||
if(pfxGetXYZMax(rayAABB,axis) < pfxGetXYZMin(proxy,axis)) {
|
||||
return;
|
||||
}
|
||||
|
||||
PfxVector3 boundOnRay = ray.m_startPosition + out.m_variable * ray.m_direction;
|
||||
PfxVector3 AABBmin = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMin(proxy),(PfxInt32)pfxGetYMin(proxy),(PfxInt32)pfxGetZMin(proxy)),center,half);
|
||||
PfxVector3 AABBmax = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMax(proxy),(PfxInt32)pfxGetYMax(proxy),(PfxInt32)pfxGetZMax(proxy)),center,half);
|
||||
|
||||
if(boundOnRay[axis] < AABBmin[axis]) {
|
||||
return;
|
||||
}
|
||||
|
||||
// スキップ
|
||||
if(pfxGetXYZMax(proxy,axis) < pfxGetXYZMin(rayAABB,axis)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
PfxUInt16 rigidbodyId = pfxGetObjectId(proxy);
|
||||
PfxUInt32 contactFilterSelf = pfxGetSelf(proxy);
|
||||
PfxUInt32 contactFilterTarget = pfxGetTarget(proxy);
|
||||
|
||||
#ifdef SCE_PFX_USE_GEOMETRY
|
||||
PfxFloatInVec t_(1.0f);
|
||||
PfxGeomAabb aabb((PfxPoint3)AABBmin,(PfxPoint3)AABBmax);
|
||||
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
|
||||
intersectionPoint(segment,aabb,&t_) && t_ < out.m_variable ) {
|
||||
#else
|
||||
float t_=1.0f;
|
||||
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
|
||||
pfxIntersectRayAABBFast(ray.m_startPosition,ray.m_direction,(AABBmax+AABBmin)*0.5f,(AABBmax-AABBmin)*0.5f,t_) && t_ < out.m_variable ) {
|
||||
#endif
|
||||
|
||||
PfxRigidState &state = offsetRigidStates[rigidbodyId];
|
||||
PfxCollidable &coll = offsetCollidables[rigidbodyId];
|
||||
PfxTransform3 transform(state.getOrientation(), state.getPosition());
|
||||
|
||||
PfxRayOutput tout = out;
|
||||
|
||||
PfxShapeIterator itrShape(coll);
|
||||
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
|
||||
const PfxShape &shape = *itrShape;
|
||||
PfxTransform3 shapeTr = transform * shape.getOffsetTransform();
|
||||
|
||||
if(pfxGetIntersectRayFunc(shape.getType())(ray,tout,shape,shapeTr) && tout.m_variable < out.m_variable) {
|
||||
out = tout;
|
||||
out.m_shapeId = j;
|
||||
out.m_objectId = rigidbodyId;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void pfxRayTraverseBackward(
|
||||
const PfxRayInput &ray,PfxRayOutput &out,const PfxAabb16 &rayAABB,
|
||||
PfxBroadphaseProxy *proxies,int numProxies,
|
||||
PfxRigidState *offsetRigidStates,
|
||||
PfxCollidable *offsetCollidables,
|
||||
int axis,const PfxVector3 ¢er,const PfxVector3 &half)
|
||||
{
|
||||
#ifdef SCE_PFX_USE_GEOMETRY
|
||||
PfxGeomSegment segment((PfxPoint3)ray.m_startPosition,ray.m_direction);
|
||||
#endif
|
||||
|
||||
for(int i=numProxies-1;i>=0;i--) {
|
||||
PfxBroadphaseProxy &proxy = proxies[i];
|
||||
|
||||
// 終了条件のチェック
|
||||
if(pfxGetXYZMax(proxy,axis) < pfxGetXYZMin(rayAABB,axis)) {
|
||||
return;
|
||||
}
|
||||
|
||||
PfxVector3 boundOnRay = ray.m_startPosition + out.m_variable * ray.m_direction;
|
||||
PfxVector3 AABBmin = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMin(proxy),(PfxInt32)pfxGetYMin(proxy),(PfxInt32)pfxGetZMin(proxy)),center,half);
|
||||
PfxVector3 AABBmax = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMax(proxy),(PfxInt32)pfxGetYMax(proxy),(PfxInt32)pfxGetZMax(proxy)),center,half);
|
||||
|
||||
if(AABBmax[axis] < boundOnRay[axis]) {
|
||||
return;
|
||||
}
|
||||
|
||||
// スキップ
|
||||
if(pfxGetXYZMax(rayAABB,axis) < pfxGetXYZMin(proxy,axis)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
PfxUInt16 rigidbodyId = pfxGetObjectId(proxy);
|
||||
PfxUInt32 contactFilterSelf = pfxGetSelf(proxy);
|
||||
PfxUInt32 contactFilterTarget = pfxGetTarget(proxy);
|
||||
|
||||
#ifdef SCE_PFX_USE_GEOMETRY
|
||||
PfxFloatInVec t_(1.0f);
|
||||
PfxGeomAabb aabb((PfxPoint3)AABBmin,(PfxPoint3)AABBmax);
|
||||
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
|
||||
intersectionPoint(segment,aabb,&t_) && t_ < out.m_variable ) {
|
||||
#else
|
||||
float t_=1.0f;
|
||||
if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&
|
||||
pfxIntersectRayAABBFast(ray.m_startPosition,ray.m_direction,(AABBmax+AABBmin)*0.5f,(AABBmax-AABBmin)*0.5f,t_) && t_ < out.m_variable ) {
|
||||
#endif
|
||||
|
||||
PfxRigidState &state = offsetRigidStates[rigidbodyId];
|
||||
PfxCollidable &coll = offsetCollidables[rigidbodyId];
|
||||
PfxTransform3 transform(state.getOrientation(), state.getPosition());
|
||||
|
||||
PfxRayOutput tout = out;
|
||||
|
||||
PfxShapeIterator itrShape(coll);
|
||||
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
|
||||
const PfxShape &shape = *itrShape;
|
||||
PfxTransform3 shapeTr = transform * shape.getOffsetTransform();
|
||||
|
||||
if(pfxGetIntersectRayFunc(shape.getType())(ray,tout,shape,shapeTr) && tout.m_variable < out.m_variable) {
|
||||
out = tout;
|
||||
out.m_shapeId = j;
|
||||
out.m_objectId = rigidbodyId;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void pfxCastSingleRay(const PfxRayInput &ray,PfxRayOutput &out,const PfxRayCastParam ¶m)
|
||||
{
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX));
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY));
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ));
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb));
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb));
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb));
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates));
|
||||
SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables));
|
||||
|
||||
PfxBroadphaseProxy *proxies[] = {
|
||||
param.proxiesX,
|
||||
param.proxiesY,
|
||||
param.proxiesZ,
|
||||
param.proxiesXb,
|
||||
param.proxiesYb,
|
||||
param.proxiesZb,
|
||||
};
|
||||
|
||||
out.m_variable = 1.0f;
|
||||
out.m_contactFlag = false;
|
||||
|
||||
// 探索軸
|
||||
PfxVector3 chkAxisVec = absPerElem(ray.m_direction);
|
||||
int axis = 0;
|
||||
if(chkAxisVec[1] < chkAxisVec[0]) axis = 1;
|
||||
if(chkAxisVec[2] < chkAxisVec[axis]) axis = 2;
|
||||
|
||||
// レイのAABB作成
|
||||
PfxVector3 p1 = ray.m_startPosition;
|
||||
PfxVector3 p2 = ray.m_startPosition + ray.m_direction;
|
||||
PfxVecInt3 rayMin,rayMax;
|
||||
pfxConvertCoordWorldToLocal(param.rangeCenter,param.rangeExtent,minPerElem(p1,p2),maxPerElem(p1,p2),rayMin,rayMax);
|
||||
|
||||
PfxAabb16 rayAABB;
|
||||
pfxSetXMin(rayAABB,rayMin.getX());
|
||||
pfxSetXMax(rayAABB,rayMax.getX());
|
||||
pfxSetYMin(rayAABB,rayMin.getY());
|
||||
pfxSetYMax(rayAABB,rayMax.getY());
|
||||
pfxSetZMin(rayAABB,rayMin.getZ());
|
||||
pfxSetZMax(rayAABB,rayMax.getZ());
|
||||
|
||||
// AABB探索開始
|
||||
int sign = ray.m_direction[axis] < 0.0f ? -1 : 1; // 探索方向
|
||||
|
||||
if(sign > 0) {
|
||||
pfxRayTraverseForward(
|
||||
ray,out,rayAABB,
|
||||
proxies[axis],param.numProxies,
|
||||
param.offsetRigidStates,param.offsetCollidables,
|
||||
axis,param.rangeCenter,param.rangeExtent);
|
||||
}
|
||||
else {
|
||||
pfxRayTraverseBackward(
|
||||
ray,out,rayAABB,
|
||||
proxies[axis+3],param.numProxies,
|
||||
param.offsetRigidStates,param.offsetCollidables,
|
||||
axis,param.rangeCenter,param.rangeExtent);
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace PhysicsEffects
|
||||
} //namespace sce
|
||||
Reference in New Issue
Block a user