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

Extras/PhysicsEffects/src/CMakeLists.txt

@@ -0,0 +1,11 @@
+SUBDIRS( 
+	base_level 
+	low_level 
+	util 
+)
+
+SUBDIRS(
+	../../../src/BulletDynamics
+	../../../src/BulletCollision
+	../../../src/LinearMath
+)

Extras/PhysicsEffects/src/base_level/CMakeLists.txt

@@ -0,0 +1,77 @@
+INCLUDE_DIRECTORIES( ${PHYSICS_EFFECTS_SOURCE_DIR}/include  )
+
+SET(PfxBaseLevel_SRCS
+						broadphase/pfx_update_broadphase_proxy.cpp
+						collision/pfx_collidable.cpp
+						collision/pfx_contact_box_box.cpp
+						collision/pfx_contact_box_capsule.cpp
+						collision/pfx_contact_box_sphere.cpp
+						collision/pfx_contact_cache.cpp
+						collision/pfx_contact_capsule_capsule.cpp
+						collision/pfx_contact_capsule_sphere.cpp
+						collision/pfx_contact_large_tri_mesh.cpp
+						collision/pfx_contact_manifold.cpp
+						collision/pfx_contact_sphere_sphere.cpp
+						collision/pfx_contact_tri_mesh_box.cpp
+						collision/pfx_contact_tri_mesh_capsule.cpp
+						collision/pfx_contact_tri_mesh_convex.cpp
+						collision/pfx_contact_tri_mesh_cylinder.cpp
+						collision/pfx_contact_tri_mesh_sphere.cpp
+						collision/pfx_gjk_solver.cpp
+						collision/pfx_gjk_support_func.cpp
+						collision/pfx_intersect_ray_box.cpp
+						collision/pfx_intersect_ray_capsule.cpp
+						collision/pfx_intersect_ray_convex.cpp
+						collision/pfx_intersect_ray_cylinder.cpp
+						collision/pfx_intersect_ray_large_tri_mesh.cpp
+						collision/pfx_intersect_ray_sphere.cpp
+						collision/pfx_shape.cpp
+						collision/pfx_simplex_solver.cpp
+						solver/pfx_contact_constraint.cpp
+						solver/pfx_joint_ball.cpp
+						solver/pfx_joint_fix.cpp
+						solver/pfx_joint_hinge.cpp
+						solver/pfx_joint_slider.cpp
+						solver/pfx_joint_swing_twist.cpp
+						solver/pfx_joint_universal.cpp
+						sort/pfx_sort.cpp
+			
+)
+
+SET(PfxBaseLevel_HDRS
+						broadphase/pfx_check_collidable.h
+						collision/pfx_contact_box_box.h
+						collision/pfx_contact_box_capsule.h
+						collision/pfx_contact_box_sphere.h
+						collision/pfx_contact_cache.h
+						collision/pfx_contact_capsule_capsule.h
+						collision/pfx_contact_capsule_sphere.h
+						collision/pfx_contact_large_tri_mesh.h
+						collision/pfx_contact_sphere_sphere.h
+						collision/pfx_contact_tri_mesh_box.h
+						collision/pfx_contact_tri_mesh_capsule.h
+						collision/pfx_contact_tri_mesh_convex.h
+						collision/pfx_contact_tri_mesh_cylinder.h
+						collision/pfx_contact_tri_mesh_sphere.h
+						collision/pfx_gjk_solver.h
+						collision/pfx_gjk_support_func.h
+						collision/pfx_intersect_common.h
+						collision/pfx_intersect_ray_box.h
+						collision/pfx_intersect_ray_capsule.h
+						collision/pfx_intersect_ray_convex.h
+						collision/pfx_intersect_ray_cylinder.h
+						collision/pfx_intersect_ray_large_tri_mesh.h
+						collision/pfx_intersect_ray_sphere.h
+						collision/pfx_mesh_common.h
+						collision/pfx_simplex_solver.h
+						solver/pfx_check_solver.h
+						solver/pfx_constraint_row_solver.h
+)
+
+
+
+
+ADD_LIBRARY(PfxBaseLevel ${PfxBaseLevel_SRCS} ${PfxBaseLevel_HDRS})
+
+SET_TARGET_PROPERTIES(PfxBaseLevel PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(PfxBaseLevel PROPERTIES SOVERSION ${BULLET_VERSION})

Extras/PhysicsEffects/src/base_level/broadphase/pfx_check_collidable.h

@@ -0,0 +1,80 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CHECK_COLLIDABLE_H
+#define _SCE_PFX_CHECK_COLLIDABLE_H
+
+#include "../../../include/physics_effects/base_level/rigidbody/pfx_rigid_state.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_aabb.h"
+#include "../../../include/physics_effects/base_level/broadphase/pfx_broadphase_proxy.h"
+#include "../../../include/physics_effects/base_level/broadphase/pfx_broadphase_pair.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+// Collidable check table
+/*
+	-----------------MotionTypeA
+	|0	1	0	1	1
+	|1	1	1	1	1
+	|0	1	0	1	1
+	|1	1	1	0	1
+	|1	1	1	1	0
+MotionTypeB
+ */
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxCheckCollidableTable(ePfxMotionType i,ePfxMotionType j)
+{
+	const PfxUInt32 collidableTable = 0x00bfafbe;
+
+	SCE_PFX_ASSERT(i < kPfxMotionTypeCount);
+	SCE_PFX_ASSERT(j < kPfxMotionTypeCount);
+
+	PfxUInt32 idx = j * kPfxMotionTypeCount + i;
+	PfxUInt32 mask = 1 << (kPfxMotionTypeCount*kPfxMotionTypeCount-1-idx);
+
+	return (collidableTable & mask) != 0;
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxCheckCollidableInBroadphase(const PfxBroadphaseProxy &proxyA, const PfxBroadphaseProxy &proxyB)
+{
+	ePfxMotionType motionA = (ePfxMotionType)(pfxGetMotionMask(proxyA)&SCE_PFX_MOTION_MASK_TYPE);
+	ePfxMotionType motionB = (ePfxMotionType)(pfxGetMotionMask(proxyB)&SCE_PFX_MOTION_MASK_TYPE);
+
+	return
+		pfxCheckCollidableTable(motionA,motionB) && // モーションタイプ別衝突判定テーブル
+		((pfxGetSelf(proxyA)&pfxGetTarget(proxyB)) && (pfxGetTarget(proxyA)&pfxGetSelf(proxyB))) &&	// 衝突フィルター
+		pfxTestAabb(proxyA,proxyB); // AABB交差判定
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxCheckCollidableInCollision(const PfxBroadphasePair &pair)
+{
+	PfxUInt32 motionA = pfxGetMotionMaskA(pair)&SCE_PFX_MOTION_MASK_TYPE;
+	PfxUInt32 motionB = pfxGetMotionMaskB(pair)&SCE_PFX_MOTION_MASK_TYPE;
+	PfxUInt32 sleepA = pfxGetMotionMaskA(pair)&SCE_PFX_MOTION_MASK_SLEEPING;
+	PfxUInt32 sleepB = pfxGetMotionMaskB(pair)&SCE_PFX_MOTION_MASK_SLEEPING;
+
+	return
+		pfxCheckCollidableTable((ePfxMotionType)motionA,(ePfxMotionType)motionB) && // モーションタイプ別衝突判定テーブル
+		!((sleepA != 0 && sleepB != 0) || (sleepA != 0 && motionB == kPfxMotionTypeFixed) || (sleepB != 0 && motionA == kPfxMotionTypeFixed)); // スリープ時のチェック
+}
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CHECK_COLLIDABLE_H

Extras/PhysicsEffects/src/base_level/broadphase/pfx_update_broadphase_proxy.cpp

@@ -0,0 +1,234 @@
+/*
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_aabb.h"
+#include "../../../include/physics_effects/base_level/broadphase/pfx_update_broadphase_proxy.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+static SCE_PFX_FORCE_INLINE
+PfxBool operator < (const PfxVector3 &v1,const PfxVector3 &v2)
+{
+	return maxElem(v2-v1) > 0.0f;
+}
+
+PfxInt32 pfxUpdateBroadphaseProxy(
+	PfxBroadphaseProxy &proxy,
+	const PfxRigidState &state,
+	const PfxCollidable &coll,
+	const PfxVector3 &worldCenter,
+	const PfxVector3 &worldExtent,
+	PfxUInt32 axis)
+{
+	SCE_PFX_ALWAYS_ASSERT(axis<3);
+	
+	PfxInt32 ret = SCE_PFX_OK;
+	
+	PfxVector3 center = state.getPosition() + coll.getCenter();
+	PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
+	
+	PfxVector3 minRig = center - half;
+	PfxVector3 maxRig = center + half;
+	
+	PfxVector3 minWld = worldCenter - worldExtent;
+	PfxVector3 maxWld = worldCenter + worldExtent;
+	
+	if(maxWld < minRig || maxRig < minWld) {
+		ret = SCE_PFX_ERR_OUT_OF_WORLD;
+	}
+
+	PfxVecInt3 aabbMin,aabbMax;
+	pfxConvertCoordWorldToLocal(worldCenter,worldExtent,minRig,maxRig,aabbMin,aabbMax);
+	
+	pfxSetXMin(proxy,aabbMin.getX());
+	pfxSetXMax(proxy,aabbMax.getX());
+	pfxSetYMin(proxy,aabbMin.getY());
+	pfxSetYMax(proxy,aabbMax.getY());
+	pfxSetZMin(proxy,aabbMin.getZ());
+	pfxSetZMax(proxy,aabbMax.getZ());
+	pfxSetKey(proxy,aabbMin.get(axis));
+	pfxSetObjectId(proxy,state.getRigidBodyId());
+	pfxSetMotionMask(proxy,state.getMotionMask());
+	pfxSetSelf(proxy,state.getContactFilterSelf());
+	pfxSetTarget(proxy,state.getContactFilterTarget());
+	
+	return ret;
+}
+
+PfxInt32 pfxUpdateBroadphaseProxy(
+	PfxBroadphaseProxy &proxy,
+	const PfxRigidState &state,
+	const PfxVector3 &objectCenter,
+	const PfxVector3 &objectHalf,
+	const PfxVector3 &worldCenter,
+	const PfxVector3 &worldExtent,
+	PfxUInt32 axis)
+{
+	SCE_PFX_ALWAYS_ASSERT(axis<3);
+	
+	PfxInt32 ret = SCE_PFX_OK;
+	
+	PfxVector3 minRig = objectCenter - objectHalf;
+	PfxVector3 maxRig = objectCenter + objectHalf;
+	
+	PfxVector3 minWld = worldCenter - worldExtent;
+	PfxVector3 maxWld = worldCenter + worldExtent;
+	
+	if(maxWld < minRig || maxRig < minWld) {
+		ret = SCE_PFX_ERR_OUT_OF_WORLD;
+	}
+	
+	PfxVecInt3 aabbMin,aabbMax;
+	pfxConvertCoordWorldToLocal(worldCenter,worldExtent,minRig,maxRig,aabbMin,aabbMax);
+
+	pfxSetXMin(proxy,aabbMin.getX());
+	pfxSetXMax(proxy,aabbMax.getX());
+	pfxSetYMin(proxy,aabbMin.getY());
+	pfxSetYMax(proxy,aabbMax.getY());
+	pfxSetZMin(proxy,aabbMin.getZ());
+	pfxSetZMax(proxy,aabbMax.getZ());
+	pfxSetKey(proxy,aabbMin.get(axis));
+	pfxSetObjectId(proxy,state.getRigidBodyId());
+	pfxSetMotionMask(proxy,state.getMotionMask());
+	pfxSetSelf(proxy,state.getContactFilterSelf());
+	pfxSetTarget(proxy,state.getContactFilterTarget());
+	
+	return ret;
+}
+
+PfxInt32 pfxUpdateBroadphaseProxy(
+	PfxBroadphaseProxy &proxyX,
+	PfxBroadphaseProxy &proxyY,
+	PfxBroadphaseProxy &proxyZ,
+	PfxBroadphaseProxy &proxyXb,
+	PfxBroadphaseProxy &proxyYb,
+	PfxBroadphaseProxy &proxyZb,
+	const PfxRigidState &state,
+	const PfxCollidable &coll,
+	const PfxVector3 &worldCenter,
+	const PfxVector3 &worldExtent)
+{
+	PfxInt32 ret = SCE_PFX_OK;
+	
+	PfxVector3 center = state.getPosition() + coll.getCenter();
+	PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
+	
+	PfxVector3 minRig = center - half;
+	PfxVector3 maxRig = center + half;
+	
+	PfxVector3 minWld = worldCenter - worldExtent;
+	PfxVector3 maxWld = worldCenter + worldExtent;
+	
+	if(maxWld < minRig || maxRig < minWld) {
+		ret = SCE_PFX_ERR_OUT_OF_WORLD;
+	}
+	
+	PfxVecInt3 aabbMin,aabbMax;
+	pfxConvertCoordWorldToLocal(worldCenter,worldExtent,minRig,maxRig,aabbMin,aabbMax);
+
+	PfxBroadphaseProxy proxy;
+	
+	pfxSetXMin(proxy,aabbMin.getX());
+	pfxSetXMax(proxy,aabbMax.getX());
+	pfxSetYMin(proxy,aabbMin.getY());
+	pfxSetYMax(proxy,aabbMax.getY());
+	pfxSetZMin(proxy,aabbMin.getZ());
+	pfxSetZMax(proxy,aabbMax.getZ());
+	pfxSetObjectId(proxy,state.getRigidBodyId());
+	pfxSetMotionMask(proxy,state.getMotionMask());
+	pfxSetSelf(proxy,state.getContactFilterSelf());
+	pfxSetTarget(proxy,state.getContactFilterTarget());
+	
+	proxyX  = proxy;
+	proxyXb = proxy;
+	proxyY  = proxy;
+	proxyYb = proxy;
+	proxyZ  = proxy;
+	proxyZb = proxy;
+	
+	pfxSetKey(proxyX ,aabbMin.getX());
+	pfxSetKey(proxyXb,aabbMax.getX());
+	pfxSetKey(proxyY ,aabbMin.getY());
+	pfxSetKey(proxyYb,aabbMax.getY());
+	pfxSetKey(proxyZ ,aabbMin.getZ());
+	pfxSetKey(proxyZb,aabbMax.getZ());
+	
+	return ret;
+}
+
+PfxInt32 pfxUpdateBroadphaseProxy(
+	PfxBroadphaseProxy &proxyX,
+	PfxBroadphaseProxy &proxyY,
+	PfxBroadphaseProxy &proxyZ,
+	PfxBroadphaseProxy &proxyXb,
+	PfxBroadphaseProxy &proxyYb,
+	PfxBroadphaseProxy &proxyZb,
+	const PfxRigidState &state,
+	const PfxVector3 &objectCenter,
+	const PfxVector3 &objectHalf,
+	const PfxVector3 &worldCenter,
+	const PfxVector3 &worldExtent)
+{
+	PfxInt32 ret = SCE_PFX_OK;
+	
+	PfxVector3 minRig = objectCenter - objectHalf;
+	PfxVector3 maxRig = objectCenter + objectHalf;
+	
+	PfxVector3 minWld = worldCenter - worldExtent;
+	PfxVector3 maxWld = worldCenter + worldExtent;
+	
+	if(maxWld < minRig || maxRig < minWld) {
+		ret = SCE_PFX_ERR_OUT_OF_WORLD;
+	}
+	
+	PfxVecInt3 aabbMin,aabbMax;
+	pfxConvertCoordWorldToLocal(worldCenter,worldExtent,minRig,maxRig,aabbMin,aabbMax);
+
+	PfxBroadphaseProxy proxy;
+	
+	pfxSetXMin(proxy,aabbMin.getX());
+	pfxSetXMax(proxy,aabbMax.getX());
+	pfxSetYMin(proxy,aabbMin.getY());
+	pfxSetYMax(proxy,aabbMax.getY());
+	pfxSetZMin(proxy,aabbMin.getZ());
+	pfxSetZMax(proxy,aabbMax.getZ());
+	pfxSetObjectId(proxy,state.getRigidBodyId());
+	pfxSetMotionMask(proxy,state.getMotionMask());
+	pfxSetSelf(proxy,state.getContactFilterSelf());
+	pfxSetTarget(proxy,state.getContactFilterTarget());
+	
+	proxyX  = proxy;
+	proxyXb = proxy;
+	proxyY  = proxy;
+	proxyYb = proxy;
+	proxyZ  = proxy;
+	proxyZb = proxy;
+	
+	pfxSetKey(proxyX ,aabbMin.getX());
+	pfxSetKey(proxyXb,aabbMax.getX());
+	pfxSetKey(proxyY ,aabbMin.getY());
+	pfxSetKey(proxyYb,aabbMax.getY());
+	pfxSetKey(proxyZ ,aabbMin.getZ());
+	pfxSetKey(proxyZb,aabbMax.getZ());
+	
+	return ret;
+}
+
+} // namespace PhysicsEffects
+} // namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_collidable.cpp

@@ -0,0 +1,56 @@
+/*
+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/collision/pfx_collidable.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+void PfxCollidable::addShape(const PfxShape &shape)
+{
+	if(m_numShapes<m_maxShapes) {
+		PfxShape &newShape = getNewShape();
+		newShape = shape;
+	}
+}
+
+void PfxCollidable::finish()
+{
+	if(m_numShapes == 0) return;
+
+	// compute AABB
+	PfxVector3 halfMax(-SCE_PFX_FLT_MAX),halfMin(SCE_PFX_FLT_MAX);
+
+	for(PfxUInt32 i=0;i<getNumShapes();i++) {
+		const PfxShape &shape = getShape(i);
+		PfxVector3 aabbMin,aabbMax;
+		shape.getAabb(aabbMin,aabbMax);
+		halfMax = maxPerElem(halfMax,aabbMax);
+		halfMin = minPerElem(halfMin,aabbMin);
+	}
+	
+	PfxVector3 allCenter = ( halfMin + halfMax ) * 0.5f;
+	PfxVector3 allHalf = ( halfMax - halfMin ) * 0.5f;
+	m_center[0] = allCenter[0];
+	m_center[1] = allCenter[1];
+	m_center[2] = allCenter[2];
+	m_half[0] = allHalf[0];
+	m_half[1] = allHalf[1];
+	m_half[2] = allHalf[2];
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_box.h

@@ -0,0 +1,33 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_BOX_BOX_H
+#define _SCE_PFX_CONTACT_BOX_BOX_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+namespace sce {
+namespace PhysicsEffects {
+
+PfxFloat pfxContactBoxBox(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_BOX_BOX_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_capsule.cpp

@@ -0,0 +1,608 @@
+/*
+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/collision/pfx_box.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_capsule.h"
+#include "pfx_contact_box_capsule.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+enum BoxCapsSepAxisType
+{
+	BOX_AXIS, CROSS_AXIS
+};
+
+//-------------------------------------------------------------------------------------------------
+// voronoiTol: bevels Voronoi planes slightly which helps when features are parallel.
+//-------------------------------------------------------------------------------------------------
+
+static const PfxFloat voronoiTol = -1.0e-5f;
+
+//-------------------------------------------------------------------------------------------------
+// lenSqrTol: minimum square of length for safe normalize.
+//-------------------------------------------------------------------------------------------------
+
+static const PfxFloat lenSqrTol = 1.0e-30f;
+
+//-------------------------------------------------------------------------------------------------
+// separating axis tests: gaps along each axis are computed, and the axis with the maximum
+// gap is stored.  cross product axes are normalized.
+//-------------------------------------------------------------------------------------------------
+
+#define AaxisTest( dim, letter, first )                                                         \
+{                                                                                               \
+   if ( first )                                                                                 \
+   {                                                                                            \
+      maxGap = gapsA.get##letter();                                                             \
+      if ( maxGap - capsuleB.m_radius > distanceThreshold ) return maxGap - capsuleB.m_radius;      \
+      axisType = BOX_AXIS;                                                                      \
+      faceDimA = dim;                                                                           \
+      axisA = ident[dim];                                                          \
+   }                                                                                            \
+   else                                                                                         \
+   {                                                                                            \
+      PfxFloat gap = gapsA.get##letter();                                                          \
+      if ( gap - capsuleB.m_radius > distanceThreshold ) return gap - capsuleB.m_radius;            \
+      else if ( gap > maxGap )                                                                  \
+      {                                                                                         \
+         maxGap = gap;                                                                          \
+         axisType = BOX_AXIS;                                                                   \
+         faceDimA = dim;                                                                        \
+         axisA = ident[dim];                                                       \
+      }                                                                                         \
+   }                                                                                            \
+}
+
+#define CrossAxisTest( dima, lettera )                                                          \
+{                                                                                               \
+   const PfxFloat lsqr_tolerance = 1.0e-30f;                                                       \
+   PfxFloat lsqr;                                                                                  \
+                                                                                                \
+   lsqr = lsqrs.get##lettera();                                                                 \
+                                                                                                \
+   if ( lsqr > lsqr_tolerance )                                                                 \
+   {                                                                                            \
+      PfxFloat l_recip = 1.0f / sqrtf( lsqr );                                                     \
+      PfxFloat gap = PfxFloat(gapsAxB.get##lettera()) * l_recip;                                      \
+                                                                                                \
+      if ( gap - capsuleB.m_radius > distanceThreshold )                                          \
+      {                                                                                         \
+         return gap - capsuleB.m_radius;                                                          \
+      }                                                                                         \
+                                                                                                \
+      if ( gap > maxGap )                                                                       \
+      {                                                                                         \
+         maxGap = gap;                                                                          \
+         axisType = CROSS_AXIS;                                                                 \
+         edgeDimA = dima;                                                                       \
+         axisA = crossProdMat.getCol##dima() * l_recip;                                         \
+      }                                                                                         \
+   }                                                                                            \
+}
+
+//-------------------------------------------------------------------------------------------------
+// tests whether a vertex of box B and a face of box A are the closest features
+//-------------------------------------------------------------------------------------------------
+
+inline
+PfxFloat
+VertexBFaceATest(
+	PfxBool& inVoronoi,
+	PfxFloat& t0,
+	PfxFloat& t1,
+	PfxVector3& ptsVec,
+	const PfxVector3& hA,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,
+	PfxFloat signB,
+	PfxFloat scaleB )
+{
+	// compute endpoint of capsule in box's coordinate system
+
+	PfxVector3 endpoint = PfxVector3( offsetAB + capsDirection * scaleB );
+
+	// compute the parameters of the point on the box face closest to this corner.
+
+	t0 = endpoint[0];
+	t1 = endpoint[1];
+
+	if ( t0 > hA[0] )
+		t0 = hA[0];
+	else if ( t0 < -hA[0] )
+		t0 = -hA[0];
+	if ( t1 > hA[1] )
+		t1 = hA[1];
+	else if ( t1 < -hA[1] )
+		t1 = -hA[1];
+
+	// get vector from face point to capsule endpoint
+
+	endpoint[0] -= t0;
+	endpoint[1] -= t1;
+	ptsVec = PfxVector3(endpoint);
+
+	// do the Voronoi test: already know the point on B is in the Voronoi region of the
+	// point on A, check the reverse.
+
+	inVoronoi = ( -signB * dot(ptsVec,capsDirection) >= voronoiTol );
+
+	return (lengthSqr(ptsVec));
+}
+
+#define VertexBFaceA_SetNewMin()                \
+{                                               \
+   minDistSqr = distSqr;                        \
+   closestPtsVec = ptsVec;                      \
+   localPointA.setX(t0);                        \
+   localPointA.setY(t1);                        \
+   segmentParamB = scaleB;                      \
+}
+
+void
+VertexBFaceATests(
+	PfxBool& done,
+	PfxFloat& minDistSqr,
+	PfxVector3& closestPtsVec,
+	PfxPoint3& localPointA,
+	PfxFloat& segmentParamB,
+	const PfxVector3& hA,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,
+	PfxFloat signB, PfxFloat scaleB,
+	PfxBool first )
+{
+	PfxVector3 ptsVec;
+	PfxFloat t0, t1;
+	PfxFloat distSqr;
+
+	// test endpoint of capsule nearest to face
+
+	distSqr = VertexBFaceATest( done, t0, t1, ptsVec, hA, offsetAB, capsDirection, signB, scaleB );
+
+	if ( first ) {
+		VertexBFaceA_SetNewMin();
+	} else {
+		if ( distSqr < minDistSqr ) {
+			VertexBFaceA_SetNewMin();
+		}
+	}
+
+	if ( done )
+		return;
+
+	signB = -signB;
+	scaleB = -scaleB;
+
+	// test other endpoint if necessary
+
+	distSqr = VertexBFaceATest( done, t0, t1, ptsVec, hA, offsetAB, capsDirection, signB, scaleB );
+
+	if ( distSqr < minDistSqr ) {
+		VertexBFaceA_SetNewMin();
+	}
+}
+
+//-------------------------------------------------------------------------------------------------
+// EdgeEdgeTest:
+//
+// tests whether a pair of edges are the closest features
+//
+// note on the shorthand:
+// 'a' & 'b' refer to the edges.
+// 'c' is the dimension of the axis that points from the face center to the edge Center
+// 'd' is the dimension of the edge Direction
+// the dimension of the face normal is 2
+//-------------------------------------------------------------------------------------------------
+
+#define EdgeEdgeTest( ac, ac_letter, ad, ad_letter )                                            \
+{                                                                                               \
+   /* get vector between edge centers */                                                        \
+                                                                                                \
+   ptsVec = offsetAB;                                                                           \
+   ptsVec.set##ac_letter( ptsVec.get##ac_letter() - scalesA.get##ac_letter() );                 \
+                                                                                                \
+   /* find parameters of closest points on line segments. */                                    \
+                                                                                                \
+   PfxFloat capsDirection_ad = capsDirection.get##ad_letter();                                     \
+   PfxFloat ptsVec_ad = ptsVec.get##ad_letter();                                                   \
+   PfxFloat capsDirDotPtsVec = dot(capsDirection,ptsVec);                                          \
+   PfxFloat denom = 1.0f - capsDirection_ad * capsDirection_ad;                                    \
+                                                                                                \
+   if ( denom == 0.0f )                                                                         \
+   {                                                                                            \
+      tA = 0.0f;                                                                                \
+   }                                                                                            \
+   else                                                                                         \
+   {                                                                                            \
+      tA = ( ptsVec_ad - capsDirDotPtsVec * capsDirection_ad ) / denom;                         \
+      if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \
+      else if ( tA > hA[ad] ) tA = hA[ad];                                                      \
+   }                                                                                            \
+                                                                                                \
+   tB = tA * capsDirection_ad - capsDirDotPtsVec;                                               \
+                                                                                                \
+   if ( tB < -hB )                                                                              \
+   {                                                                                            \
+      tB = -hB;                                                                                 \
+      tA = tB * capsDirection_ad + ptsVec_ad;                                                   \
+                                                                                                \
+      if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \
+      else if ( tA > hA[ad] ) tA = hA[ad];                                                      \
+   }                                                                                            \
+   else if ( tB > hB )                                                                          \
+   {                                                                                            \
+      tB = hB;                                                                                  \
+      tA = tB * capsDirection_ad + ptsVec_ad;                                                   \
+                                                                                                \
+      if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \
+      else if ( tA > hA[ad] ) tA = hA[ad];                                                      \
+   }                                                                                            \
+                                                                                                \
+   /* make vector to point at tB on edge B from the center of edge A. */                        \
+   /* test that it lies inside edge A's voronoi region. */                                      \
+                                                                                                \
+   ptsVec += capsDirection * tB;                                                                \
+                                                                                                \
+   PfxVector3 cptsVec( mulPerElem( ptsVec, signsA ) );                                             \
+                                                                                                \
+   inVoronoi = ( cptsVec[ac] >= voronoiTol * cptsVec[2] ) &&                                    \
+               ( cptsVec[2] >= voronoiTol * cptsVec[ac] );                                      \
+                                                                                                \
+   ptsVec.set##ad_letter( ptsVec.get##ad_letter() - tA );                                       \
+                                                                                                \
+   return lengthSqr(ptsVec);                                                                    \
+}
+
+PfxFloat
+EdgeEdgeTest_01(
+	PfxBool& inVoronoi,
+	PfxFloat& tA,
+	PfxFloat& tB,
+	PfxVector3& ptsVec,
+	const PfxVector3& hA,
+	PfxFloat hB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA )
+{
+	EdgeEdgeTest( 0, X, 1, Y );
+}
+
+PfxFloat
+EdgeEdgeTest_10(
+	PfxBool& inVoronoi,
+	PfxFloat& tA,
+	PfxFloat& tB,
+	PfxVector3& ptsVec,
+	const PfxVector3& hA,
+	PfxFloat hB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA )
+{
+	EdgeEdgeTest( 1, Y, 0, X );
+}
+
+#define EdgeEdge_SetNewMin( ac_letter, ad_letter )                         \
+{                                                                          \
+   minDistSqr = distSqr;                                                   \
+   closestPtsVec = ptsVec;                                                 \
+   localPointA.set##ac_letter(scalesA.get##ac_letter());                   \
+   localPointA.set##ad_letter(tA);                                         \
+   segmentParamB = tB;                                                     \
+   otherFaceDimA = testOtherFaceDimA;                                      \
+}
+
+void
+EdgeEdgeTests(
+	PfxBool& done,
+	PfxFloat& minDistSqr,
+	PfxVector3& closestPtsVec,
+	PfxPoint3& localPointA,
+	PfxFloat& segmentParamB,
+	int & otherFaceDimA,
+	const PfxVector3& hA,
+	PfxFloat hB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,
+	PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,
+	PfxBool first )
+{
+	PfxVector3 ptsVec;
+	PfxFloat tA, tB;
+	int testOtherFaceDimA;
+
+	testOtherFaceDimA = 0;
+
+	PfxFloat distSqr = EdgeEdgeTest_01( done, tA, tB, ptsVec, hA, hB,
+									 offsetAB, capsDirection, signsA, scalesA );
+
+	if ( first ) {
+		EdgeEdge_SetNewMin( X, Y );
+	} else {
+		if ( distSqr < minDistSqr ) {
+			EdgeEdge_SetNewMin( X, Y );
+		}
+	}
+
+	if ( done )
+		return;
+
+	signsA.setX( -signsA.getX() );
+	scalesA.setX( -scalesA.getX() );
+
+	distSqr = EdgeEdgeTest_01( done, tA, tB, ptsVec, hA, hB,
+							   offsetAB, capsDirection, signsA, scalesA );
+
+	if ( distSqr < minDistSqr ) {
+		EdgeEdge_SetNewMin( X, Y );
+	}
+
+	if ( done )
+		return;
+
+	testOtherFaceDimA = 1;
+
+	distSqr = EdgeEdgeTest_10( done, tA, tB, ptsVec, hA, hB,
+							   offsetAB, capsDirection, signsA, scalesA );
+
+	if ( distSqr < minDistSqr ) {
+		EdgeEdge_SetNewMin( Y, X );
+	}
+
+	if ( done )
+		return;
+
+	signsA.setY( -signsA.getY() );
+	scalesA.setY( -scalesA.getY() );
+
+	distSqr = EdgeEdgeTest_10( done, tA, tB, ptsVec, hA, hB,
+							   offsetAB, capsDirection, signsA, scalesA );
+
+	if ( distSqr < minDistSqr ) {
+		EdgeEdge_SetNewMin( Y, X );
+	}
+}
+
+PfxFloat pfxContactBoxCapsule(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	PfxBox boxA = *((PfxBox*)shapeA);
+	PfxCapsule capsuleB = *((PfxCapsule*)shapeB);
+
+	PfxVector3 ident[3] = {
+		PfxVector3(1.0,0.0,0.0),
+		PfxVector3(0.0,1.0,0.0),
+		PfxVector3(0.0,0.0,1.0),
+	};
+
+	// get capsule position and direction in box's coordinate system
+
+	PfxMatrix3 matrixA = transformA.getUpper3x3();
+	PfxMatrix3 matrixAinv = transpose(matrixA);
+
+	PfxVector3 directionB = transformB.getUpper3x3().getCol0();
+	PfxVector3 translationB = transformB.getTranslation();
+
+	PfxVector3 capsDirection = matrixAinv * directionB;
+	PfxVector3 absCapsDirection = absPerElem(capsDirection);
+	PfxVector3 offsetAB = matrixAinv * (translationB - transformA.getTranslation());
+
+	// find separating axis with largest gap between projections
+
+	BoxCapsSepAxisType axisType;
+	PfxVector3 axisA;
+	PfxFloat maxGap;
+	int faceDimA = 0, edgeDimA = 0;
+
+	// face axes
+
+	// can compute all the gaps at once with VU0
+
+	PfxVector3 gapsA = absPerElem(offsetAB) - boxA.m_half - absCapsDirection * capsuleB.m_halfLen;
+
+	AaxisTest( 0, X, true );
+	AaxisTest( 1, Y, false );
+	AaxisTest( 2, Z, false );
+
+	// cross product axes
+
+	// compute gaps on all cross product axes using some VU0 math.  suppose there's a tradeoff
+	// between doing this with SIMD all at once or without SIMD in each cross product test, since
+	// some test might exit early.
+
+	PfxVector3 lsqrs, projOffset, projAhalf;
+
+	PfxMatrix3 crossProdMat = crossMatrix(capsDirection) * PfxMatrix3::identity();
+	PfxMatrix3 crossProdMatT = crossMatrix(-capsDirection) * PfxMatrix3::identity();
+
+	lsqrs = mulPerElem( crossProdMatT.getCol0(), crossProdMatT.getCol0() ) +
+			mulPerElem( crossProdMatT.getCol1(), crossProdMatT.getCol1() ) +
+			mulPerElem( crossProdMatT.getCol2(), crossProdMatT.getCol2() );
+
+	projOffset = crossProdMatT * offsetAB;
+	projAhalf = absPerElem(crossProdMatT) * boxA.m_half;
+
+	PfxVector3 gapsAxB = absPerElem(projOffset) - projAhalf;
+
+	CrossAxisTest( 0, X );
+	CrossAxisTest( 1, Y );
+	CrossAxisTest( 2, Z );
+
+	// make axis point from box center towards capsule center.
+
+	if ( dot(axisA,offsetAB) < 0.0f )
+		axisA = -axisA;
+
+	// find the face on box whose normal best matches the separating axis. will use the entire
+	// face only in degenerate cases.
+	//
+	// to make things simpler later, change the coordinate system so that the face normal is the z
+	// direction.  if an edge cross product axis was chosen above, also align the box edge to the y
+	// axis.  this saves the later tests from having to know which face was chosen.  changing the
+	// coordinate system involves permuting vector elements, so construct a permutation matrix.
+	// I believe this is a faster way to permute a bunch of vectors than using arrays.
+
+	int dimA[3];
+
+	if ( axisType == CROSS_AXIS ) {
+		PfxVector3 absAxisA = PfxVector3(absPerElem(axisA));
+
+		dimA[1] = edgeDimA;
+
+		if ( edgeDimA == 0 ) {
+			if ( absAxisA[1] > absAxisA[2] ) {
+				dimA[0] = 2;
+				dimA[2] = 1;
+			} else                             {
+				dimA[0] = 1;
+				dimA[2] = 2;
+			}
+		} else if ( edgeDimA == 1 ) {
+			if ( absAxisA[2] > absAxisA[0] ) {
+				dimA[0] = 0;
+				dimA[2] = 2;
+			} else                             {
+				dimA[0] = 2;
+				dimA[2] = 0;
+			}
+		} else {
+			if ( absAxisA[0] > absAxisA[1] ) {
+				dimA[0] = 1;
+				dimA[2] = 0;
+			} else                             {
+				dimA[0] = 0;
+				dimA[2] = 1;
+			}
+		}
+	} else {
+		dimA[2] = faceDimA;
+		dimA[0] = (faceDimA+1)%3;
+		dimA[1] = (faceDimA+2)%3;
+	}
+
+	PfxMatrix3 aperm_col;
+
+	aperm_col.setCol0(ident[dimA[0]]);
+	aperm_col.setCol1(ident[dimA[1]]);
+	aperm_col.setCol2(ident[dimA[2]]);
+
+	PfxMatrix3 aperm_row = transpose(aperm_col);
+
+	// permute vectors to be in face coordinate system.
+
+	PfxVector3 offsetAB_perm = aperm_row * offsetAB;
+	PfxVector3 halfA_perm = aperm_row * boxA.m_half;
+	PfxVector3 signsA_perm = copySignPerElem(PfxVector3(1.0f), aperm_row * axisA);
+	PfxVector3 scalesA_perm = mulPerElem( signsA_perm, halfA_perm );
+	PfxVector3 capsDirection_perm = aperm_row * capsDirection;
+	PfxFloat signB = (-dot(capsDirection,axisA) > 0.0f)? 1.0f : -1.0f;
+	PfxFloat scaleB = signB * capsuleB.m_halfLen;
+
+	// compute the vector between the center of the box face and the capsule center
+
+	offsetAB_perm.setZ( offsetAB_perm.getZ() - scalesA_perm.getZ() );
+
+	// if box and capsule overlap, this will separate them for finding points of penetration.
+
+	if ( maxGap < 0.0f ) {
+		offsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;
+	}
+
+	// for each vertex/face or edge/edge pair of box face and line segment, find the closest
+	// points.
+	//
+	// these points each have an associated feature (vertex, edge, or face).  if each
+	// point is in the external Voronoi region of the other's feature, they are the
+	// closest points of the objects, and the algorithm can exit.
+	//
+	// the feature pairs are arranged so that in the general case, the first test will
+	// succeed.  degenerate cases (line segment parallel to face) may require up to all tests
+	// in the worst case.
+	//
+	// if for some reason no case passes the Voronoi test, the features with the minimum
+	// distance are returned.
+
+	PfxVector3 closestPtsVec_perm;
+	PfxPoint3 localPointA_perm;
+	PfxFloat minDistSqr;
+	PfxFloat segmentParamB;
+	PfxBool done;
+
+	localPointA_perm.setZ( scalesA_perm.getZ() );
+	scalesA_perm.setZ(0.0f);
+
+	PfxVector3 hA_perm( halfA_perm );
+
+	int otherFaceDimA;
+
+	if ( axisType == CROSS_AXIS ) {
+		EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
+					   otherFaceDimA,
+					   hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm,
+					   scalesA_perm, true );
+
+		if ( !done ) {
+			VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
+							   hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, false );
+		}
+	} else {
+		VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
+						   hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, true );
+
+		if ( !done ) {
+			EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
+						   otherFaceDimA,
+						   hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm,
+						   scalesA_perm, false );
+		}
+	}
+
+	// compute normal
+
+	PfxBool centerInside = ( signsA_perm.getZ() * closestPtsVec_perm.getZ() < 0.0f );
+
+	if ( centerInside || ( minDistSqr < lenSqrTol ) ) {
+		normal = matrixA * axisA;
+	} else {
+		PfxVector3 closestPtsVec = aperm_col * closestPtsVec_perm;
+		normal = matrixA * ( closestPtsVec * (1.0f/sqrtf( minDistSqr )) );
+	}
+
+	// compute box point
+
+	pointA = PfxPoint3( aperm_col * PfxVector3( localPointA_perm ) );
+
+	// compute capsule point
+
+	pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( directionB * segmentParamB - normal * capsuleB.m_radius ) );
+
+	if ( centerInside ) {
+		return (-sqrtf( minDistSqr ) - capsuleB.m_radius);
+	} else {
+		return (sqrtf( minDistSqr ) - capsuleB.m_radius);
+	}
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_capsule.h

@@ -0,0 +1,34 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_BOX_CAPSULE_H
+#define _SCE_PFX_CONTACT_BOX_CAPSULE_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxFloat pfxContactBoxCapsule(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif //  _SCE_PFX_CONTACT_BOX_CAPSULE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_sphere.cpp

@@ -0,0 +1,236 @@
+/*
+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/collision/pfx_box.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_sphere.h"
+#include "pfx_contact_box_sphere.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+static const PfxFloat lenSqrTol = 1.0e-30f;
+
+inline
+PfxFloat
+VertexBFaceATest(
+	PfxVector3& ptsVec,
+	PfxFloat& t0,
+	PfxFloat& t1,
+	const PfxVector3& hA,
+	const PfxVector3 &offsetAB )
+{
+	// compute center of sphere in box's coordinate system
+
+	PfxVector3 cptsVec = PfxVector3(offsetAB);
+
+	// compute the parameters of the point on the face
+
+	t0 = cptsVec[0];
+	t1 = cptsVec[1];
+
+	if ( t0 > hA[0] )
+		t0 = hA[0];
+	else if ( t0 < -hA[0] )
+		t0 = -hA[0];
+	if ( t1 > hA[1] )
+		t1 = hA[1];
+	else if ( t1 < -hA[1] )
+		t1 = -hA[1];
+
+	cptsVec[0] -= t0;
+	cptsVec[1] -= t1;
+
+	ptsVec = PfxVector3( cptsVec );
+
+	return dot(ptsVec,ptsVec);
+}
+
+PfxFloat pfxContactBoxSphere(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	PfxBox &boxA = *((PfxBox*)shapeA);
+	PfxSphere &sphereB = *((PfxSphere*)shapeB);
+	
+	PfxVector3 ident[3] = {
+		PfxVector3(1.0,0.0,0.0),
+		PfxVector3(0.0,1.0,0.0),
+		PfxVector3(0.0,0.0,1.0),
+	};
+	
+	//{
+	//	PfxMatrix3 identity = PfxMatrix3::identity();
+	//	ident[0] = identity.getCol0();
+	//	ident[1] = identity.getCol1();
+	//	ident[2] = identity.getCol2();
+	//}
+
+	// offsetAB is vector from A's center to B's center, in A's coordinate system
+
+	PfxVector3 translationB = transformB.getTranslation();
+	PfxVector3 offsetAB = transpose(transformA.getUpper3x3()) * ( translationB -
+					   transformA.getTranslation() );
+
+	// find separating axis with largest gap between objects
+
+	PfxVector3 axisA;
+	int   faceDimA;
+	PfxFloat maxGap;
+
+	PfxVector3 gapsA = absPerElem(offsetAB) - boxA.m_half - PfxVector3(sphereB.m_radius);
+	PfxVector3 signsA = copySignPerElem(PfxVector3(1.0f),offsetAB);
+
+	{
+		PfxFloat gap = gapsA[0];
+
+		if( gap > distanceThreshold ) {
+			return gap;
+		}
+
+		maxGap = gap;
+		faceDimA = 0;
+		axisA = mulPerElem( ident[0], signsA );
+
+		if( gap > maxGap ) {
+			maxGap = gap;
+			faceDimA = 0;
+			axisA = mulPerElem( ident[0], signsA );
+		}
+
+		gap = gapsA[1];
+
+		if( gap > distanceThreshold ) {
+			return gap;
+		}
+
+		if( gap > maxGap ) {
+			maxGap = gap;
+			faceDimA = 1;
+			axisA = mulPerElem( ident[1], signsA );
+		}
+
+		gap = gapsA[2];
+
+		if( gap > distanceThreshold ) {
+			return gap;
+		}
+
+		if( gap > maxGap ) {
+			maxGap = gap;
+			faceDimA = 2;
+			axisA = mulPerElem( ident[2], signsA );
+		}
+	}
+
+	// choose face in this direction, and make a new coordinate system which the z axis = face
+	// normal, x and y axes tangent to the face.  to transform vectors into this coordinate
+	// system, will use a permutation matrix.
+
+	int dimA[3];
+
+	dimA[2] = faceDimA;
+	dimA[0] = (faceDimA+1)%3;
+	dimA[1] = (faceDimA+2)%3;
+
+	PfxMatrix3 apermCol;
+	
+	apermCol.setCol0(ident[dimA[0]]);
+	apermCol.setCol1(ident[dimA[1]]);
+	apermCol.setCol2(ident[dimA[2]]);
+
+	PfxMatrix3 apermRow = transpose(apermCol);
+
+	// permute vectors
+
+	PfxVector3 halfA_perm = apermRow * boxA.m_half;
+	PfxVector3 offsetAB_perm = apermRow * offsetAB;
+	PfxVector3 signsA_perm = apermRow * signsA;
+
+	// compute the vector between the center of the box face and the sphere center
+
+	PfxFloat signA2 = signsA_perm.getZ();
+	PfxFloat scaleA2 = halfA_perm.getZ() * signA2;
+	offsetAB_perm.setZ( offsetAB_perm.getZ() - scaleA2 );
+
+	// find point on face closest to sphere center
+
+	PfxFloat t0, t1;
+	PfxFloat minDistSqr;
+	PfxVector3 closestPtsVec_perm;
+	PfxPoint3 localPointA_perm;
+
+	minDistSqr = VertexBFaceATest( closestPtsVec_perm, t0, t1, PfxVector3( halfA_perm ), offsetAB_perm );
+
+	//SCE_PFX_PRINTF("faceDimA %d dimA %d %d %d\n",faceDimA,dimA[0],dimA[1],dimA[2]);
+	//SCE_PFX_PRINTF("boxA.m_half %f %f %f\n",boxA.m_half[0],boxA.m_half[1],boxA.m_half[2]);
+	//SCE_PFX_PRINTF("ident %f %f %f | %f %f %f | %f %f %f\n",
+	//	ident[0][0],ident[0][1],ident[0][2],
+	//	ident[1][0],ident[1][1],ident[1][2],
+	//	ident[2][0],ident[2][1],ident[2][2]);
+	//SCE_PFX_PRINTF("apermCol %f %f %f | %f %f %f | %f %f %f\n",
+	//	apermCol[0][0],apermCol[0][1],apermCol[0][2],
+	//	apermCol[1][0],apermCol[1][1],apermCol[1][2],
+	//	apermCol[2][0],apermCol[2][1],apermCol[2][2]);
+	//SCE_PFX_PRINTF("apermRow %f %f %f | %f %f %f | %f %f %f\n",
+	//	apermRow[0][0],apermRow[0][1],apermRow[0][2],
+	//	apermRow[1][0],apermRow[1][1],apermRow[1][2],
+	//	apermRow[2][0],apermRow[2][1],apermRow[2][2]);
+
+	//SCE_PFX_PRINTF("closestPtsVec_perm %f %f %f\n",closestPtsVec_perm[0],closestPtsVec_perm[1],closestPtsVec_perm[2]);
+	//SCE_PFX_PRINTF("halfA_perm %f %f %f\n",halfA_perm[0],halfA_perm[1],halfA_perm[2]);
+	//SCE_PFX_PRINTF("offsetAB_perm %f %f %f\n",offsetAB_perm[0],offsetAB_perm[1],offsetAB_perm[2]);
+	//SCE_PFX_PRINTF("t0 %f t1 %f scaleA2 %f\n",t0,t1,scaleA2);
+
+	//SCE_PFX_PRINTF("minDistSqr %f sphereB.m_radius %f\n",minDistSqr,sphereB.m_radius);
+
+	localPointA_perm = PfxPoint3( t0, t1, scaleA2 );
+
+	// compute normal
+
+	bool centerInside = ( signA2 * closestPtsVec_perm.getZ() < 0.0f );
+
+	if ( centerInside || ( minDistSqr < lenSqrTol ) ) {
+		normal = transformA * axisA;
+	} else {
+		PfxVector3 closestPtsVec = apermCol * closestPtsVec_perm;
+		normal = transformA * ( closestPtsVec * ( 1.0f / sqrtf( minDistSqr ) ) );
+	}
+
+	// compute box point
+
+	pointA = PfxPoint3( apermCol * PfxVector3( localPointA_perm ) );
+
+	// compute sphere point
+
+	pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -normal * sphereB.m_radius ) );
+
+	// return distance
+
+	//SCE_PFX_PRINTF("normal %f %f %f\n",(float)normal[0],(float)normal[1],(float)normal[2]);
+	//SCE_PFX_PRINTF("pointA %f %f %f\n",(float)pointA[0],(float)pointA[1],(float)pointA[2]);
+	//SCE_PFX_PRINTF("pointB %f %f %f\n",(float)pointB[0],(float)pointB[1],(float)pointB[2]);
+
+	if ( centerInside ) {
+		return -sqrtf( minDistSqr ) - sphereB.m_radius;
+	} else {
+		return sqrtf( minDistSqr ) - sphereB.m_radius;
+	}
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_sphere.h

@@ -0,0 +1,34 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_BOX_SPHERE_H
+#define _SCE_PFX_CONTACT_BOX_SPHERE_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxFloat pfxContactBoxSphere(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_BOXSPHERE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_cache.cpp

@@ -0,0 +1,164 @@
+/*
+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 "pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_CONTACT_SAME_POINT 0.01f
+
+int PfxContactCache::findNearestContactPoint(const PfxPoint3 &newPoint,const PfxVector3 &newNormal)
+{
+	int nearestIdx = -1;
+	PfxFloat minDiff = SCE_PFX_CONTACT_SAME_POINT;
+	for(PfxUInt32 i=0;i<m_numContacts;i++) {
+		PfxVector3 dist = m_cachedContactPoints[i].m_localPointA-newPoint;
+		PfxFloat diff = lengthSqr(dist);
+		if(diff < minDiff && dot(newNormal,m_cachedContactPoints[i].m_normal) > 0.99f) {
+			minDiff = diff;
+			nearestIdx = i;
+		}
+	}
+	return nearestIdx;
+}
+
+int PfxContactCache::sort4ContactPoints(const PfxPoint3 &newCP,PfxFloat newDistance)
+{
+	int maxPenetrationIndex = -1;
+	PfxFloat maxPenetration = newDistance;
+
+	// 最も深い衝突点は排除対象からはずす
+	for(int i=0;i<SCE_PFX_MAX_CACHED_CONTACT_POINTS;i++) {
+		if(m_cachedContactPoints[i].m_distance < maxPenetration) {
+			maxPenetrationIndex = i;
+			maxPenetration = m_cachedContactPoints[i].m_distance;
+		}
+	}
+	
+	PfxFloat res[4] = {0.0f};
+	
+	// 各点を除いたときの衝突点が作る面積のうち、最も大きくなるものを選択
+	PfxVector3 newp(newCP);
+	PfxVector3 p[4];
+	p[0] = (PfxVector3)m_cachedContactPoints[0].m_localPointA;
+	p[1] = (PfxVector3)m_cachedContactPoints[1].m_localPointA;
+	p[2] = (PfxVector3)m_cachedContactPoints[2].m_localPointA;
+	p[3] = (PfxVector3)m_cachedContactPoints[3].m_localPointA;
+
+	if(maxPenetrationIndex != 0) {
+		PfxVector3 a0 = newp-p[1];
+		PfxVector3 b0 = p[3]-p[2];
+		res[0] = lengthSqr(cross(a0,b0));
+	}
+ 
+	if(maxPenetrationIndex != 1) {
+		PfxVector3 a1 = newp-p[0];
+		PfxVector3 b1 = p[3]-p[2];
+		res[1] = lengthSqr(cross(a1,b1));
+	}
+
+	if(maxPenetrationIndex != 2) {
+		PfxVector3 a2 = newp-p[0];
+		PfxVector3 b2 = p[3]-p[1];
+		res[2] = lengthSqr(cross(a2,b2));
+	}
+
+	if(maxPenetrationIndex != 3) {
+		PfxVector3 a3 = newp-p[0];
+		PfxVector3 b3 = p[2]-p[1];
+		res[3] = lengthSqr(cross(a3,b3));
+	}
+
+	int maxIndex = 0;
+	PfxFloat maxVal = res[0];
+
+	if (res[1] > maxVal) {
+		maxIndex = 1;
+		maxVal = res[1];
+	}
+
+	if (res[2] > maxVal) {
+		maxIndex = 2;
+		maxVal = res[2];
+	}
+
+	if (res[3] > maxVal) {
+		maxIndex = 3;
+		maxVal = res[3];
+	}
+
+	return maxIndex;
+}
+
+void PfxContactCache::addContactPoint(
+		PfxFloat newDistance,
+		const PfxVector3 &newNormal, // world coords
+		const PfxPoint3 &newPointA, // local to the objectA
+		const PfxPoint3 &newPointB, // local to the objectB
+		PfxSubData subData)
+{
+	int id = findNearestContactPoint(newPointA,newNormal);
+
+	if(id < 0 && m_numContacts < SCE_PFX_MAX_CACHED_CONTACT_POINTS) {
+		// 衝突点を新規追加
+		id = m_numContacts++;
+		m_cachedContactPoints[id].reset();
+	}
+	else if(id < 0){
+		// ソート
+		id = sort4ContactPoints(newPointA,newDistance);
+		m_cachedContactPoints[id].reset();
+	}
+
+	m_cachedContactPoints[id].m_distance = newDistance;
+	m_cachedContactPoints[id].m_subData = subData;
+	m_cachedContactPoints[id].m_normal = newNormal;
+	m_cachedContactPoints[id].m_localPointA = newPointA;
+	m_cachedContactPoints[id].m_localPointB = newPointB;
+}
+
+void PfxContactCache::addContactPoint(const PfxCachedContactPoint &cp)
+{
+	PfxPoint3 pA = cp.m_localPointA;
+	
+	int id = findNearestContactPoint(pA,cp.m_normal);
+	
+	if(id >= 0) {
+		if(m_cachedContactPoints[id].m_distance > cp.m_distance) {
+			// 同一点を発見、衝突点情報を更新
+			m_cachedContactPoints[id].m_distance = cp.m_distance;
+			m_cachedContactPoints[id].m_normal = cp.m_normal;
+			m_cachedContactPoints[id].m_localPointA = cp.m_localPointA;
+			m_cachedContactPoints[id].m_localPointB = cp.m_localPointB;
+		}
+	}
+	else if(m_numContacts < SCE_PFX_MAX_CACHED_CONTACT_POINTS) {
+		// 衝突点を新規追加
+		m_cachedContactPoints[m_numContacts++] = cp;
+	}
+	else {
+		// ソート
+		id = sort4ContactPoints(pA,cp.m_distance);
+		
+		// コンタクトポイント入れ替え
+		m_cachedContactPoints[id] = cp;
+	}
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_cache.h

@@ -0,0 +1,100 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_CACHE_H
+#define _SCE_PFX_CONTACT_CACHE_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+#include "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_sub_data.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_MAX_CACHED_CONTACT_POINTS 4
+
+/*
+	内部の衝突判定に使う軽量なコンタクトキャッシュ
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// Contact Point
+
+struct PfxCachedContactPoint
+{
+	PfxSubData m_subData;
+	PfxUInt8 m_shapeIdA;
+	PfxUInt8 m_shapeIdB;
+	SCE_PFX_PADDING(1,2)
+	PfxFloat m_distance;
+	PfxVector3 m_normal;
+	PfxPoint3 m_localPointA;
+	PfxPoint3 m_localPointB;
+
+	void reset()
+	{
+		m_shapeIdA = m_shapeIdB = 0;
+		m_subData = PfxSubData();
+		m_distance = SCE_PFX_FLT_MAX;
+	}
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Contact Point
+
+class PfxContactCache
+{
+private:
+	PfxUInt32 m_numContacts;
+	SCE_PFX_PADDING(1,12)
+	PfxCachedContactPoint m_cachedContactPoints[SCE_PFX_MAX_CACHED_CONTACT_POINTS];
+	
+	int findNearestContactPoint(const PfxPoint3 &newPoint,const PfxVector3 &newNormal);
+	int sort4ContactPoints(const PfxPoint3 &newPoint,PfxFloat newDistance);
+	
+public:
+	PfxContactCache() : m_numContacts(0) {}
+	
+	void addContactPoint(
+		PfxFloat newDistance,
+		const PfxVector3 &newNormal, // world normal vector
+		const PfxPoint3 &newPointA, // local contact point to the objectA
+		const PfxPoint3 &newPointB, // local contact point to the objectB
+		PfxSubData m_subData);
+	
+	void addContactPoint(const PfxCachedContactPoint &cp);
+	
+	int getNumContacts() const {return (int)m_numContacts;}
+	
+	PfxCachedContactPoint &getContactPoint(int i) {return m_cachedContactPoints[i];}
+	
+	const PfxCachedContactPoint &getContactPoint(int i) const {return m_cachedContactPoints[i];}
+
+	PfxFloat getDistance(int i) {return m_cachedContactPoints[i].m_distance;}
+	
+	const PfxVector3 &getNormal(int i) const {return m_cachedContactPoints[i].m_normal;}
+	
+	const PfxPoint3 &getLocalPointA(int i) const {return m_cachedContactPoints[i].m_localPointA;}
+	
+	const PfxPoint3 &getLocalPointB(int i) const {return m_cachedContactPoints[i].m_localPointB;}
+	
+	const PfxSubData &getSubData(int i) const {return m_cachedContactPoints[i].m_subData;}
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_CACHE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_capsule_capsule.cpp

@@ -0,0 +1,144 @@
+/*
+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/base_level/collision/pfx_capsule.h"
+#include "pfx_contact_capsule_capsule.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+inline
+void
+segmentsClosestPoints(
+	PfxVector3& ptsVector,
+	PfxVector3& offsetA,
+	PfxVector3& offsetB,
+	PfxFloat& tA, PfxFloat& tB,
+	const PfxVector3 &translation,
+	const PfxVector3 &dirA, PfxFloat hlenA,
+	const PfxVector3 &dirB, PfxFloat hlenB )
+{
+	// compute the parameters of the closest points on each line segment
+
+	PfxFloat dirA_dot_dirB = dot(dirA,dirB);
+	PfxFloat dirA_dot_trans = dot(dirA,translation);
+	PfxFloat dirB_dot_trans = dot(dirB,translation);
+
+	PfxFloat denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+	if ( denom == 0.0f ) {
+		tA = 0.0f;
+	} else {
+		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+	if ( tB < -hlenB ) {
+		tB = -hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	} else if ( tB > hlenB ) {
+		tB = hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	// compute the closest points relative to segment centers.
+
+	offsetA = dirA * tA;
+	offsetB = dirB * tB;
+
+	ptsVector = translation - offsetA + offsetB;
+}
+
+inline
+void
+segmentsNormal( PfxVector3& normal, const PfxVector3 &ptsVector )
+{
+	// compute the unit direction vector between the closest points.
+	// with convex objects, you want the unit direction providing the largest gap between the
+	// objects when they're projected onto it.  So, if you have a few candidates covering different
+	// configurations of the objects, you can compute them all, test the gaps and pick best axis
+	// based on this.  Some directions might be degenerate, and the normalized() function tests for
+	// degeneracy and returns an arbitrary unit vector in that case.
+
+	PfxVector3 testDir;
+
+	// closest points vector
+
+	normal = pfxSafeNormalize(ptsVector);
+}
+
+PfxFloat pfxContactCapsuleCapsule(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	PfxCapsule &capsuleA = *((PfxCapsule*)shapeA);
+	PfxCapsule &capsuleB = *((PfxCapsule*)shapeB);
+
+	PfxVector3 directionA = transformA.getUpper3x3().getCol0();
+	PfxVector3 translationA = transformA.getTranslation();
+	PfxVector3 directionB = transformB.getUpper3x3().getCol0();
+	PfxVector3 translationB = transformB.getTranslation();
+
+	// translation between centers
+
+	PfxVector3 translation = translationB - translationA;
+
+	// compute the closest points of the capsule line segments
+
+	PfxVector3 ptsVector;           // the vector between the closest points
+	PfxVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
+	PfxFloat tA, tB;              // parameters on line segment
+
+	segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
+						   directionA, capsuleA.m_halfLen, directionB, capsuleB.m_halfLen );
+
+	PfxFloat distance = length(ptsVector) - capsuleA.m_radius - capsuleB.m_radius;
+
+	if ( distance > distanceThreshold )
+		return distance;
+
+	// compute the contact normal
+
+	segmentsNormal( normal, ptsVector );
+
+	// compute points on capsules
+
+	pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) );
+	pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( offsetB - normal * capsuleB.m_radius ) );
+
+	return distance;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_capsule_capsule.h

@@ -0,0 +1,34 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_CAPSULE_CAPSULE_H
+#define _SCE_PFX_CONTACT_CAPSULE_CAPSULE_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxFloat pfxContactCapsuleCapsule(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_CAPSULE_CAPSULE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_capsule_sphere.cpp

@@ -0,0 +1,108 @@
+/*
+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/base_level/collision/pfx_capsule.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_sphere.h"
+#include "pfx_contact_capsule_sphere.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+inline
+void
+segmentPointClosestPoints(
+	PfxVector3& ptsVector,
+	PfxVector3& offsetA,
+	PfxFloat& tA,
+	const PfxVector3 &translation,
+	const PfxVector3 &dirA, PfxFloat hLenA )
+{
+	// compute the parameters of the closest points on each line segment
+
+	tA = dot(dirA,translation);
+
+	if ( tA < -hLenA )
+		tA = -hLenA;
+	else if ( tA > hLenA )
+		tA = hLenA;
+
+	// compute the closest point on segment relative to its center.
+
+	offsetA = dirA * tA;
+	ptsVector = translation - offsetA;
+}
+
+inline
+void
+segmentPointNormal(     PfxVector3& normal, const  PfxVector3 &ptsVector )
+{
+	// compute the unit direction vector between the closest points.
+	// with convex objects, you want the unit direction providing the largest gap between the
+	// objects when they're projected onto it.  So, if you have a few candidates covering different
+	// configurations of the objects, you can compute them all, test the gaps and pick best axis
+	// based on this.  Some directions might be degenerate, and the normalized() function tests for
+	// degeneracy and returns an arbitrary unit vector in that case.
+
+	// closest points vector
+
+	normal = pfxSafeNormalize(ptsVector);
+}
+
+PfxFloat pfxContactCapsuleSphere(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	PfxCapsule &capsuleA = *((PfxCapsule*)shapeA);
+	PfxSphere &sphereB = *((PfxSphere*)shapeB);
+
+	PfxVector3 directionA = transformA.getUpper3x3().getCol0();
+	PfxVector3 translationA = transformA.getTranslation();
+	PfxVector3 translationB = transformB.getTranslation();
+
+	// translation between centers of capsule and sphere
+
+	PfxVector3 translation = translationB - translationA;
+
+	// compute the closest point on the capsule line segment to the sphere center
+
+	PfxVector3 ptsVector;
+	PfxVector3 offsetA;
+	PfxFloat tA;
+
+	segmentPointClosestPoints( ptsVector, offsetA, tA, translation, directionA, capsuleA.m_halfLen );
+
+	PfxFloat distance = length(ptsVector) - capsuleA.m_radius - sphereB.m_radius;
+
+	if ( distance > distanceThreshold )
+		return distance;
+
+	// compute the contact normal
+
+	segmentPointNormal( normal, ptsVector );
+
+	// compute points on capsule and sphere
+
+	pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) );
+	pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -normal * sphereB.m_radius ) );
+
+	return distance;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_capsule_sphere.h

@@ -0,0 +1,34 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_CAPSULE_SPHERE_H
+#define _SCE_PFX_CONTACT_CAPSULE_SPHERE_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxFloat pfxContactCapsuleSphere(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_CAPSULE_SPHERE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_large_tri_mesh.cpp

@@ -0,0 +1,152 @@
+/*
+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 "pfx_contact_tri_mesh_sphere.h"
+#include "pfx_contact_tri_mesh_box.h"
+#include "pfx_contact_tri_mesh_capsule.h"
+#include "pfx_contact_tri_mesh_cylinder.h"
+#include "pfx_contact_tri_mesh_convex.h"
+#include "pfx_contact_large_tri_mesh.h"
+#include "pfx_intersect_common.h"
+#include "pfx_mesh_common.h"
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+PfxInt32 pfxContactLargeTriMesh(
+				PfxContactCache &contacts,
+				const PfxLargeTriMesh *lmeshA,
+				const PfxTransform3 &transformA,
+				const PfxShape &shapeB,
+				const PfxTransform3 &transformB,
+				PfxFloat distanceThreshold)
+{
+	PfxTransform3 transformAB;
+	PfxMatrix3 matrixAB;
+	PfxVector3 offsetAB;
+	
+	// Bローカル→Aローカルへの変換
+	transformAB = orthoInverse(transformA) * transformB;
+	matrixAB = transformAB.getUpper3x3();
+	offsetAB = transformAB.getTranslation();
+	
+	// -----------------------------------------------------
+	// LargeTriMeshに含まれるTriMeshのAABBと凸体のAABBを判定し、
+	// 交差するものを個別に衝突判定する。※LargeMesh座標系
+	
+	PfxVector3 shapeHalf(0.0f);
+	PfxVector3 shapeCenter = offsetAB;
+	
+
+	switch(shapeB.getType()) {
+		case kPfxShapeSphere:
+		shapeHalf = PfxVector3(shapeB.getSphere().m_radius);
+		break;
+		
+		case kPfxShapeCapsule:
+		{
+			PfxCapsule capsule = shapeB.getCapsule();
+			shapeHalf = absPerElem(matrixAB) * PfxVector3(capsule.m_halfLen+capsule.m_radius,capsule.m_radius,capsule.m_radius);
+		}
+		break;
+		
+		case kPfxShapeCylinder:
+		{
+			PfxCylinder cylinder = shapeB.getCylinder();
+			shapeHalf = absPerElem(matrixAB) * PfxVector3(cylinder.m_halfLen,cylinder.m_radius,cylinder.m_radius);
+		}
+		break;
+		
+		case kPfxShapeBox:
+		shapeHalf = absPerElem(matrixAB) * shapeB.getBox().m_half;
+		break;
+		
+		case kPfxShapeConvexMesh:
+	shapeHalf = absPerElem(matrixAB) * shapeB.getConvexMesh()->m_half;
+		break;
+		
+		default:
+		break;
+	}
+
+	// -----------------------------------------------------
+	// アイランドとの衝突判定
+
+	PfxVecInt3 aabbMinL,aabbMaxL;
+	lmeshA->getLocalPosition((shapeCenter-shapeHalf),(shapeCenter+shapeHalf),aabbMinL,aabbMaxL);
+	
+	PfxUInt32 numIslands = lmeshA->m_numIslands;
+
+	{
+	for(PfxUInt32 i=0;i<numIslands;i++) {
+		// AABBチェック
+		PfxAabb16 aabbB = lmeshA->m_aabbList[i];
+		if(aabbMaxL.getX() < pfxGetXMin(aabbB) || aabbMinL.getX() > pfxGetXMax(aabbB)) continue;
+		if(aabbMaxL.getY() < pfxGetYMin(aabbB) || aabbMinL.getY() > pfxGetYMax(aabbB)) continue;
+		if(aabbMaxL.getZ() < pfxGetZMin(aabbB) || aabbMinL.getZ() > pfxGetZMax(aabbB)) continue;
+		
+		PfxTriMesh *island = &lmeshA->m_islands[i];
+
+			// 衝突判定
+			PfxContactCache localContacts;
+			switch(shapeB.getType()) {
+				case kPfxShapeSphere:
+				pfxContactTriMeshSphere(localContacts,island,transformA,shapeB.getSphere(),transformB,distanceThreshold);
+				break;
+				
+				case kPfxShapeCapsule:
+				pfxContactTriMeshCapsule(localContacts,island,transformA,shapeB.getCapsule(),transformB,distanceThreshold);
+				break;
+				
+				case kPfxShapeBox:
+				pfxContactTriMeshBox(localContacts,island,transformA,shapeB.getBox(),transformB,distanceThreshold);
+				break;
+				
+				case kPfxShapeCylinder:
+				pfxContactTriMeshCylinder(localContacts,island,transformA,shapeB.getCylinder(),transformB,distanceThreshold);
+				break;
+				
+				case kPfxShapeConvexMesh:
+			pfxContactTriMeshConvex(localContacts,island,transformA,*shapeB.getConvexMesh(),transformB,distanceThreshold);
+				break;
+				
+				default:
+				break;
+			}
+
+			
+			// 衝突点を追加
+			for(int j=0;j<localContacts.getNumContacts();j++) {
+				PfxSubData subData = localContacts.getSubData(j);
+				subData.setIslandId(i);
+				contacts.addContactPoint(
+					localContacts.getDistance(j),
+					localContacts.getNormal(j),
+					localContacts.getLocalPointA(j),
+					localContacts.getLocalPointB(j),
+					subData);
+			}
+		}
+	}
+
+
+	return contacts.getNumContacts();
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_large_tri_mesh.h

@@ -0,0 +1,38 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_LARGE_TRI_MESH_H
+#define _SCE_PFX_CONTACT_LARGE_TRI_MESH_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_large_tri_mesh.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_shape.h"
+#include "pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxContactLargeTriMesh(
+				PfxContactCache &contacts,
+				const PfxLargeTriMesh *lmeshA,
+				const PfxTransform3 &transformA,
+				const PfxShape &shapeB,
+				const PfxTransform3 &transformB,
+				PfxFloat distanceThreshold = SCE_PFX_FLT_MAX );
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_LARGE_TRI_MESH_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_manifold.cpp

@@ -0,0 +1,234 @@
+/*
+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/collision/pfx_contact_manifold.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_CONTACT_SAME_POINT			0.01f
+#define SCE_PFX_CONTACT_THRESHOLD_NORMAL	0.01f	// 衝突点の閾値（法線方向）
+#define SCE_PFX_CONTACT_THRESHOLD_TANGENT	0.002f	// 衝突点の閾値（平面上）
+
+int PfxContactManifold::findNearestContactPoint(const PfxPoint3 &newPoint,const PfxVector3 &newNormal)
+{
+	int nearestIdx = -1;
+	PfxFloat minDiff = SCE_PFX_CONTACT_SAME_POINT;
+	for(int i=0;i<m_numContacts;i++) {
+		PfxFloat diff = lengthSqr(pfxReadVector3(m_contactPoints[i].m_localPointA) - PfxVector3(newPoint));
+		if(diff < minDiff && dot(newNormal,pfxReadVector3(m_contactPoints[i].m_constraintRow[0].m_normal)) > 0.99f) {
+			minDiff = diff;
+			nearestIdx = i;
+		}
+	}
+	return nearestIdx;
+}
+
+int PfxContactManifold::sort4ContactPoints(const PfxPoint3 &newCP,PfxFloat newDistance)
+{
+	int maxPenetrationIndex = -1;
+	PfxFloat maxPenetration = newDistance;
+
+	// 最も深い衝突点は排除対象からはずす
+	for(int i=0;i<SCE_PFX_NUMCONTACTS_PER_BODIES;i++) {
+		if(m_contactPoints[i].m_distance < maxPenetration) {
+			maxPenetrationIndex = i;
+			maxPenetration = m_contactPoints[i].m_distance;
+		}
+	}
+	
+	PfxFloat res[4] = {0.0f};
+	
+	// 各点を除いたときの衝突点が作る面積のうち、最も大きくなるものを選択
+	PfxVector3 newp(newCP);
+	PfxVector3 p[4];
+	p[0] = pfxReadVector3(m_contactPoints[0].m_localPointA);
+	p[1] = pfxReadVector3(m_contactPoints[1].m_localPointA);
+	p[2] = pfxReadVector3(m_contactPoints[2].m_localPointA);
+	p[3] = pfxReadVector3(m_contactPoints[3].m_localPointA);
+
+	if(maxPenetrationIndex != 0) {
+		PfxVector3 a0 = newp-p[1];
+		PfxVector3 b0 = p[3]-p[2];
+		res[0] = lengthSqr(cross(a0,b0));
+	}
+ 
+	if(maxPenetrationIndex != 1) {
+		PfxVector3 a1 = newp-p[0];
+		PfxVector3 b1 = p[3]-p[2];
+		res[1] = lengthSqr(cross(a1,b1));
+	}
+
+	if(maxPenetrationIndex != 2) {
+		PfxVector3 a2 = newp-p[0];
+		PfxVector3 b2 = p[3]-p[1];
+		res[2] = lengthSqr(cross(a2,b2));
+	}
+
+	if(maxPenetrationIndex != 3) {
+		PfxVector3 a3 = newp-p[0];
+		PfxVector3 b3 = p[2]-p[1];
+		res[3] = lengthSqr(cross(a3,b3));
+	}
+
+	int maxIndex = 0;
+	PfxFloat maxVal = res[0];
+
+	if (res[1] > maxVal) {
+		maxIndex = 1;
+		maxVal = res[1];
+	}
+
+	if (res[2] > maxVal) {
+		maxIndex = 2;
+		maxVal = res[2];
+	}
+
+	if (res[3] > maxVal) {
+		maxIndex = 3;
+		maxVal = res[3];
+	}
+
+	return maxIndex;
+}
+
+void PfxContactManifold::addContactPoint(
+		PfxFloat newDistance,
+		const PfxVector3 &newNormal, // world coords
+		const PfxPoint3 &newPointA, // local to the objectA
+		const PfxPoint3 &newPointB, // local to the objectB
+		PfxSubData subData)
+{
+	int id = findNearestContactPoint(newPointA,newNormal);
+
+	if(id < 0 && m_numContacts < SCE_PFX_NUMCONTACTS_PER_BODIES) {
+		// 衝突点を新規追加
+		id = m_numContacts++;
+		m_contactPoints[id].reset();
+	}
+	else if(id < 0){
+		// ソート
+		id = sort4ContactPoints(newPointA,newDistance);
+		m_contactPoints[id].reset();
+	}
+
+	m_contactPoints[id].m_distance = newDistance;
+	m_contactPoints[id].m_subData = subData;
+	pfxStorePoint3(newPointA,m_contactPoints[id].m_localPointA);
+	pfxStorePoint3(newPointB,m_contactPoints[id].m_localPointB);
+	pfxStoreVector3(newNormal,m_contactPoints[id].m_constraintRow[0].m_normal);
+}
+
+void PfxContactManifold::addContactPoint(const PfxContactPoint &cp)
+{
+	PfxPoint3 pA = pfxReadPoint3(cp.m_localPointA);
+	
+	int id = findNearestContactPoint(pA,pfxReadVector3(cp.m_constraintRow[0].m_normal));
+	
+	if(id >= 0) {
+#if 1
+		PfxVector3 nml1(pfxReadVector3(m_contactPoints[id].m_constraintRow[0].m_normal));
+		PfxVector3 nml2(pfxReadVector3(cp.m_constraintRow[0].m_normal));
+		if(fabsf(dot(nml1,nml2)) > 0.99f ) {
+			// 同一点を発見、蓄積された情報を継続
+			m_contactPoints[id].m_distance = cp.m_distance;
+			m_contactPoints[id].m_localPointA[0] = cp.m_localPointA[0];
+			m_contactPoints[id].m_localPointA[1] = cp.m_localPointA[1];
+			m_contactPoints[id].m_localPointA[2] = cp.m_localPointA[2];
+			m_contactPoints[id].m_localPointB[0] = cp.m_localPointB[0];
+			m_contactPoints[id].m_localPointB[1] = cp.m_localPointB[1];
+			m_contactPoints[id].m_localPointB[2] = cp.m_localPointB[2];
+			m_contactPoints[id].m_constraintRow[0].m_normal[0] = cp.m_constraintRow[0].m_normal[0];
+			m_contactPoints[id].m_constraintRow[0].m_normal[1] = cp.m_constraintRow[0].m_normal[1];
+			m_contactPoints[id].m_constraintRow[0].m_normal[2] = cp.m_constraintRow[0].m_normal[2];
+		}
+		else {
+			// 同一点ではあるが法線が違うため更新
+			m_contactPoints[id] = cp;
+		}
+#else
+		if(m_contactPoints[id].m_distance > cp.m_distance) {
+			// 同一点を発見、衝突点情報を更新
+			m_contactPoints[id].m_distance = cp.m_distance;
+			m_contactPoints[id].m_localPointA[0] = cp.m_localPointA[0];
+			m_contactPoints[id].m_localPointA[1] = cp.m_localPointA[1];
+			m_contactPoints[id].m_localPointA[2] = cp.m_localPointA[2];
+			m_contactPoints[id].m_localPointB[0] = cp.m_localPointB[0];
+			m_contactPoints[id].m_localPointB[1] = cp.m_localPointB[1];
+			m_contactPoints[id].m_localPointB[2] = cp.m_localPointB[2];
+			m_contactPoints[id].m_constraintRow[0].m_normal[0] = cp.m_constraintRow[0].m_normal[0];
+			m_contactPoints[id].m_constraintRow[0].m_normal[1] = cp.m_constraintRow[0].m_normal[1];
+			m_contactPoints[id].m_constraintRow[0].m_normal[2] = cp.m_constraintRow[0].m_normal[2];
+		}
+#endif
+	}
+	else if(m_numContacts < SCE_PFX_NUMCONTACTS_PER_BODIES) {
+		// 衝突点を新規追加
+		m_contactPoints[m_numContacts++] = cp;
+	}
+	else {
+		// ソート
+		id = sort4ContactPoints(pA,cp.m_distance);
+		
+		// コンタクトポイント入れ替え
+		m_contactPoints[id] = cp;
+	}
+}
+
+void PfxContactManifold::merge(const PfxContactManifold &contact)
+{
+	SCE_PFX_ALWAYS_ASSERT(m_rigidBodyIdA == contact.getRigidBodyIdA());
+	SCE_PFX_ALWAYS_ASSERT(m_rigidBodyIdB == contact.getRigidBodyIdB());
+
+	for(int i=0;i<contact.getNumContacts();i++) {
+		addContactPoint(contact.getContactPoint(i));
+	}
+}
+
+void PfxContactManifold::refresh(const PfxVector3 &pA,const PfxQuat &qA,const PfxVector3 &pB,const PfxQuat &qB)
+{
+	// 衝突点の更新
+	// 両衝突点間の距離が閾値（CONTACT_THRESHOLD）を超えたら消去
+	for(int i=0;i<(int)m_numContacts;i++) {
+		if(m_contactPoints[i].m_duration > 0) {
+			PfxVector3 normal = pfxReadVector3(m_contactPoints[i].m_constraintRow[0].m_normal);
+			PfxVector3 cpA = pA + rotate(qA,pfxReadVector3(m_contactPoints[i].m_localPointA));
+			PfxVector3 cpB = pB + rotate(qB,pfxReadVector3(m_contactPoints[i].m_localPointB));
+
+			// 貫通深度がプラスに転じたかどうかをチェック
+			PfxFloat distance = dot(normal,(cpA - cpB));
+			if(distance > SCE_PFX_CONTACT_THRESHOLD_NORMAL) {
+				removeContactPoint(i);
+				i--;
+				continue;
+			}
+			m_contactPoints[i].m_distance = distance;
+
+			// 深度方向を除去して両点の距離をチェック
+			cpA = cpA - m_contactPoints[i].m_distance * normal;
+			PfxFloat distanceAB = lengthSqr(cpA - cpB);
+			if(distanceAB > SCE_PFX_CONTACT_THRESHOLD_TANGENT) {
+				removeContactPoint(i);
+				i--;
+				continue;
+			}
+		}
+		if(m_contactPoints[i].m_duration < 255) m_contactPoints[i].m_duration++;
+	}
+	if(m_numContacts > 0 && m_duration < 65535) m_duration++;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_sphere_sphere.cpp

@@ -0,0 +1,76 @@
+/*
+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/collision/pfx_sphere.h"
+#include "pfx_contact_sphere_sphere.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+const PfxFloat lenSqrTol = 1.0e-30f;
+
+PfxFloat pfxContactSphereSphere(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	PfxSphere &sphereA = *((PfxSphere*)shapeA);
+	PfxSphere &sphereB = *((PfxSphere*)shapeB);
+
+	PfxVector3 direction(0.0f);
+
+	PfxVector3 translationA = transformA.getTranslation();
+	PfxVector3 translationB = transformB.getTranslation();
+
+	// get the offset vector between sphere centers
+
+	PfxVector3 offsetAB;
+
+	offsetAB = translationB - translationA;
+
+	// normalize the offset to compute the direction vector
+
+	PfxFloat distSqr = dot(offsetAB,offsetAB);
+	PfxFloat dist = sqrtf(distSqr);
+	PfxFloat sphereDist = dist - sphereA.m_radius - sphereB.m_radius;
+
+	if ( sphereDist > distanceThreshold ) {
+		return sphereDist;
+	}
+
+	if ( distSqr > lenSqrTol ) {
+		PfxFloat distInv = 1.0f / dist;
+
+		direction = offsetAB * distInv;
+	} else {
+		direction = PfxVector3(0.0f, 0.0f, 1.0f);
+	}
+
+	normal = direction;
+
+	// compute the points on the spheres, in world space
+
+	pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( direction * sphereA.m_radius ) );
+	pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -direction * sphereB.m_radius ) );
+
+	// return the distance between the spheres
+
+	return sphereDist;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_sphere_sphere.h

@@ -0,0 +1,33 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_SPHERE_SPHERE_H
+#define _SCE_PFX_CONTACT_SPHERE_SPHERE_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+namespace sce {
+namespace PhysicsEffects {
+
+PfxFloat pfxContactSphereSphere(
+	PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
+	void *shapeA,const PfxTransform3 &transformA,
+	void *shapeB,const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_SPHERE_SPHERE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_box.cpp

@@ -0,0 +1,456 @@
+/*
+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 "pfx_contact_tri_mesh_box.h"
+#include "pfx_intersect_common.h"
+#include "pfx_mesh_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+static SCE_PFX_FORCE_INLINE
+bool checkSAT(const PfxVector3 &axis,PfxFloat AMin,PfxFloat AMax,PfxFloat BMin,PfxFloat BMax,PfxFloat &distMin,PfxVector3 &axisMin)
+{
+	// ■ 非接触.
+
+	// A:          +----+
+	// B:+----+
+	if(BMax <= AMin) {
+		return true;
+	}
+
+	// A:+----+
+	// B:          +----+
+	else if(AMax <= BMin) {
+		return true;
+	}
+
+	// ■ 内包
+
+	//A:      +--+
+	//B:   +------+
+	if(BMin < AMin && AMax < BMax) {
+		PfxFloat d = AMin-BMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = axis;
+		}
+	}
+
+	//A:   +------+
+	//B:    +--+
+	else if(AMin < BMin && BMax < AMax) {
+		PfxFloat d = AMin-BMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = axis;
+		}
+	}
+
+	// ■ 接触
+	
+	// A:   +----+
+	// B:+----+
+	else if(BMin < AMin && AMin < BMax) {
+		PfxFloat d = AMin-BMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = axis;
+		}
+	}
+
+	// A:   +----+
+	// B:      +----+
+	else if(AMin < BMin && BMin < AMax) {
+		PfxFloat d = BMin-AMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = -axis;
+		}
+	}
+
+	return false;
+}
+
+static SCE_PFX_FORCE_INLINE
+bool pfxContactTriangleBox(PfxContactCache &contacts,PfxUInt32 facetId,
+							const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
+							const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
+							PfxUInt32 edgeChk,
+							const PfxVector3 &boxHalf)
+{
+	const PfxFloat epsilon = 0.00001f;
+	
+	// 最も浅い貫通深度とそのときの分離軸
+	PfxFloat distMin = -SCE_PFX_FLT_MAX;
+	PfxVector3 axisMin(0.0f);
+
+	//-------------------------------------------
+	// １．分離軸判定
+	{
+		PfxVector3 facetPnts[6] = {
+			p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
+		};
+
+		PfxVector3 sideNml[3] = {
+			normalize(cross((facetPnts[1] - facetPnts[0]),normal)),
+			normalize(cross((facetPnts[2] - facetPnts[1]),normal)),
+			normalize(cross((facetPnts[0] - facetPnts[2]),normal)),
+		};
+
+
+		// Trianglesの面 -> Box
+		{
+			// 分離軸
+			const PfxVector3 sepAxis = normal;
+
+			// 分離平面
+			PfxPlane planeA(sepAxis,p0);
+			
+			// Boxを分離軸に投影して範囲を取得
+			PfxFloat r = dot(boxHalf,absPerElem(sepAxis));
+			PfxFloat boxOffset = planeA.onPlane(PfxVector3(0.0f));
+			PfxFloat boxMax = boxOffset + r;
+			PfxFloat boxMin = boxOffset - r;
+			
+			// 判定
+			if(boxMin > 0.0f || boxMax < -thickness) {
+				return false;
+			}
+
+			if(distMin < boxMin) {
+				distMin = boxMin;
+				axisMin = -sepAxis;
+			}
+		}
+
+		// Box -> Triangles
+		for(int bf=0;bf<3;bf++) {
+			// 分離軸
+			PfxVector3 sepAxis(0.0f);
+			sepAxis[bf] = 1.0f;
+
+			// 分離軸の方向をチェック
+			if(dot(normal,sepAxis) > 0.0f)
+				sepAxis = -sepAxis;
+
+			// Trianglesを分離軸に投影して範囲を取得
+			PfxFloat triMin,triMax;
+			pfxGetProjAxisPnts6(facetPnts,sepAxis,triMin,triMax);
+
+			// Boxを分離軸に投影して範囲を取得
+			PfxFloat boxMin = -boxHalf[bf];
+			PfxFloat boxMax =  boxHalf[bf];
+			
+			if(checkSAT(sepAxis,triMin,triMax,boxMin,boxMax,distMin,axisMin)) {
+				return false;
+			}
+		}
+
+		// エッジ Triangles面のエッジ(x3)×Boxのエッジ(x3)
+		for(int e=0;e<3;e++) {
+			PfxVector3 dir = normalize(facetPnts[(e+1)%3] - facetPnts[e]);
+
+			for(int i=0;i<3;i++) {
+				PfxVector3 boxEdge(0.0f);
+				boxEdge[i] = 1.0f;
+				
+				// エッジが平行であれば判定しない
+				if(pfxIsSameDirection(dir,boxEdge)) continue;
+
+				PfxVector3 sepAxis = normalize(cross(dir,boxEdge));
+
+				// 分離軸の方向をチェック
+				if(dot(normal,sepAxis) > 0.0f)
+					sepAxis = -sepAxis;
+
+				// Trianglesを分離軸に投影して範囲を取得
+				PfxFloat triMin,triMax;
+				pfxGetProjAxisPnts6(facetPnts,sepAxis,triMin,triMax);
+
+				// Boxを分離軸に投影して範囲を取得
+				PfxFloat r = dot(boxHalf,absPerElem(sepAxis));
+				PfxFloat boxMin = -r;
+				PfxFloat boxMax = r;
+
+				if(checkSAT(sepAxis,triMin,triMax,boxMin,boxMax,distMin,axisMin)) {
+					return false;
+				}
+			}
+		}
+
+		// 面に厚みがある場合の補助的な判定（交差するかしないかだけを判定）
+		if(thickness > 0.0f) {
+			// 厚み側面の法線
+			for(int i=0;i<3;i++) {
+				// 分離平面
+				PfxPlane planeA(sideNml[i],facetPnts[i]);
+
+				// Boxを分離軸に投影して範囲を取得
+				PfxFloat r = dot(boxHalf,absPerElem(sideNml[i]));
+				PfxFloat boxOffset = planeA.onPlane(PfxVector3(0.0f));
+				PfxFloat boxMin = boxOffset - r;
+
+				if(boxMin > 0.0f) {
+					return false;
+				}
+			}
+
+			// ２つの厚み側面のなすエッジ3×ボックスのエッジ3
+			for(int e=0;e<3;e++) {
+				PfxVector3 edgeVec = normalize(cross(sideNml[(e+1)%3],sideNml[e]));
+
+				for(int i=0;i<3;i++) {
+					PfxVector3 boxEdge(0.0f);
+					boxEdge[i] = 1.0f;
+					
+					// エッジが平行であれば判定しない
+					if(pfxIsSameDirection(edgeVec,boxEdge)) continue;
+
+					PfxVector3 sepAxis = normalize(cross(edgeVec,boxEdge));
+
+					// Trianglesを分離軸に投影して範囲を取得
+					PfxFloat triMin,triMax;
+					pfxGetProjAxisPnts3(facetPnts,sepAxis,triMin,triMax);
+
+					// Boxを分離軸に投影して範囲を取得
+					PfxFloat r = dot(boxHalf,absPerElem(sepAxis));
+					PfxFloat boxMin = -r;
+					PfxFloat boxMax = r;
+
+					if(triMax < boxMin || boxMax < triMin) {
+						return false;
+					}
+				}
+			}
+		}
+	}
+
+	//-------------------------------------------
+	// ２．衝突点の探索
+	{
+		// 分離軸方向に引き離す(最近接を判定するため、交差回避させる)
+		PfxVector3 sepAxis = 1.1f * fabsf(distMin) * axisMin;
+		
+		const PfxVector3 facetPnts[3] = {
+			p0 + sepAxis,
+			p1 + sepAxis,
+			p2 + sepAxis,
+		};
+		
+		const PfxVector3 boxPnts[8] = {
+			mulPerElem(PfxVector3(-1.0f,-1.0f,-1.0f),boxHalf),
+			mulPerElem(PfxVector3(-1.0f,-1.0f, 1.0f),boxHalf),
+			mulPerElem(PfxVector3( 1.0f,-1.0f, 1.0f),boxHalf),
+			mulPerElem(PfxVector3( 1.0f,-1.0f,-1.0f),boxHalf),
+			mulPerElem(PfxVector3(-1.0f, 1.0f,-1.0f),boxHalf),
+			mulPerElem(PfxVector3(-1.0f, 1.0f, 1.0f),boxHalf),
+			mulPerElem(PfxVector3( 1.0f, 1.0f, 1.0f),boxHalf),
+			mulPerElem(PfxVector3( 1.0f, 1.0f,-1.0f),boxHalf),
+		};
+		
+		//--------------------------------------------------------------------
+		// 衝突点の探索
+		
+		PfxClosestPoints cp;
+		PfxVector3 sA,sB;
+		
+		// エッジ間の最短距離と座標値を算出
+		{
+			const int boxIds[] = {
+				0,1,
+				1,2,
+				2,3,
+				3,0,
+				4,5,
+				5,6,
+				6,7,
+				7,4,
+				0,4,
+				3,7,
+				2,6,
+				1,5,
+			};
+
+			for(int i=0;i<3;i++) {
+				for(int j=0;j<12;j++) {
+					pfxClosestTwoLines(facetPnts[i],facetPnts[(i+1)%3],boxPnts[boxIds[j*2]],boxPnts[boxIds[j*2+1]],sA,sB);
+					cp.add(PfxPoint3(sA),PfxPoint3(sB),lengthSqr(sA-sB));
+				}
+			}
+		}
+		
+		// Triangleの頂点 -> Boxの面
+		{
+#ifdef SCE_PFX_USE_GEOMETRY
+			PfxFloatInVec sqrDist;
+			PfxGeomAabb aabb(PfxPoint3(0.0f),boxHalf);
+			{
+				PfxPoint3 closestPoint = sce::Geometry::Aos::closestPoint(PfxPoint3(facetPnts[0]),aabb,&sqrDist);
+				cp.add(PfxPoint3(facetPnts[0]),closestPoint,sqrDist);
+			}
+			{
+				PfxPoint3 closestPoint = sce::Geometry::Aos::closestPoint(PfxPoint3(facetPnts[1]),aabb,&sqrDist);
+				cp.add(PfxPoint3(facetPnts[1]),closestPoint,sqrDist);
+			}
+			{
+				PfxPoint3 closestPoint = sce::Geometry::Aos::closestPoint(PfxPoint3(facetPnts[2]),aabb,&sqrDist);
+				cp.add(PfxPoint3(facetPnts[2]),closestPoint,sqrDist);
+			}
+#else
+			pfxClosestPointAABB(facetPnts[0],boxHalf,sB);
+			cp.add(PfxPoint3(facetPnts[0]),PfxPoint3(sB),lengthSqr(sB-facetPnts[0]));
+			
+			pfxClosestPointAABB(facetPnts[1],boxHalf,sB);
+			cp.add(PfxPoint3(facetPnts[1]),PfxPoint3(sB),lengthSqr(sB-facetPnts[1]));
+			
+			pfxClosestPointAABB(facetPnts[2],boxHalf,sB);
+			cp.add(PfxPoint3(facetPnts[2]),PfxPoint3(sB),lengthSqr(sB-facetPnts[2]));
+#endif
+		}
+
+		// Boxの頂点 -> Trianglesの面
+		PfxTriangle triangleA(facetPnts[0],facetPnts[1],facetPnts[2]);
+		for(int i=0;i<8;i++) {
+			pfxClosestPointTriangle(boxPnts[i],triangleA,sA);
+			cp.add(PfxPoint3(sA),PfxPoint3(boxPnts[i]),lengthSqr(sA-boxPnts[i]));
+		}
+		
+		for(int i=0;i<cp.numPoints;i++) {
+			if(cp.distSqr[i] < cp.closestDistSqr + epsilon) {
+				cp.pA[i] -= sepAxis;
+				// 面上の最近接点が凸エッジ上でない場合は法線を変える
+				if( ((edgeChk&0x01)&&pfxPointOnLine(PfxVector3(cp.pA[i]),p0,p1)) ||
+					((edgeChk&0x02)&&pfxPointOnLine(PfxVector3(cp.pA[i]),p1,p2)) ||
+					((edgeChk&0x04)&&pfxPointOnLine(PfxVector3(cp.pA[i]),p2,p0)) ) {
+					axisMin=-normal;
+				}
+				
+				PfxSubData subData;
+				subData.setFacetId(facetId);
+				contacts.addContactPoint(-length(cp.pB[i]-cp.pA[i]),axisMin,cp.pA[i],cp.pB[i],subData);
+			}
+		}
+	}
+
+	return true;
+}
+
+// 分離軸が見つかった場合はすぐに抜けるため最短距離が返されるわけではないことに注意
+PfxInt32 pfxContactTriMeshBox(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxBox &boxB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	(void) distanceThreshold;
+
+	PfxTransform3 transformAB, transformBA;
+	PfxMatrix3 matrixBA;
+	PfxVector3 offsetBA;
+
+	// Bローカル→Aローカルへの変換
+	transformAB = orthoInverse(transformA) * transformB;
+
+	// Aローカル→Bローカルへの変換
+	transformBA = orthoInverse(transformAB);
+
+	matrixBA = transformBA.getUpper3x3();
+	offsetBA = transformBA.getTranslation();
+
+	//-------------------------------------------
+	// 判定する面を絞り込む
+
+	PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = {0};
+	PfxUInt32 numSelFacets = 0;
+
+	// ※boxB座標系
+	PfxVector3 aabbB = boxB.m_half;
+	numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
+	
+	if(numSelFacets == 0) {
+		return 0;
+	}
+	
+	//-------------------------------------------
+	// 分離軸判定(SAT)
+	// ※Box座標系 (Bローカル)で判定
+	
+	PfxContactCache localContacts;
+	
+	for(PfxUInt32 f = 0; f < numSelFacets; f++) {
+		const PfxFacet &facet = meshA->m_facets[selFacets[f]];
+		
+		PfxVector3 facetNormal = matrixBA * pfxReadVector3(facet.m_normal);
+
+		PfxVector3 facetPntsA[3] = {
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[0]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[1]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[2]],
+		};
+
+		const PfxEdge *edge[3] = {
+			&meshA->m_edges[facet.m_edgeIds[0]],
+			&meshA->m_edges[facet.m_edgeIds[1]],
+			&meshA->m_edges[facet.m_edgeIds[2]],
+		};
+		
+		PfxUInt32 edgeChk = 
+			((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |
+			((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |
+			((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);
+
+		pfxContactTriangleBox(localContacts,selFacets[f],
+							facetNormal,facetPntsA[0],facetPntsA[1],facetPntsA[2],
+							facet.m_thickness,
+							0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),
+							edgeChk,boxB.m_half);
+	}
+	
+	for(int i=0;i<localContacts.getNumContacts();i++) {
+		PfxSubData subData = localContacts.getSubData(i);
+		
+		const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
+		
+		PfxTriangle triangleA(
+			meshA->m_verts[facet.m_vertIds[0]],
+			meshA->m_verts[facet.m_vertIds[1]],
+			meshA->m_verts[facet.m_vertIds[2]]);
+
+		PfxFloat s=0.0f,t=0.0f;
+		pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);
+		subData.m_type = PfxSubData::MESH_INFO;
+		subData.setFacetLocalS(s);
+		subData.setFacetLocalT(t);
+		
+		contacts.addContactPoint(
+			localContacts.getDistance(i),
+			transformB.getUpper3x3() * localContacts.getNormal(i),
+			transformAB * localContacts.getLocalPointA(i),
+			localContacts.getLocalPointB(i),
+			subData);
+	}
+	
+	return contacts.getNumContacts();
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_box.h

@@ -0,0 +1,38 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_TRI_MESH_BOX_H
+#define _SCE_PFX_CONTACT_TRI_MESH_BOX_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_box.h"
+#include "pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxContactTriMeshBox(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxBox &boxB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX );
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_CONTACT_TRI_MESH_BOX_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_capsule.cpp

@@ -0,0 +1,570 @@
+/*
+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 "pfx_contact_tri_mesh_capsule.h"
+#include "pfx_intersect_common.h"
+#include "pfx_mesh_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+// 分離軸が見つかった場合はすぐに処理を抜けるため最短距離が返されるわけではないことに注意
+
+/*
+	○ カプセル分離軸(x19)
+	面法線
+	カプセル軸 x エッジ0
+	カプセル軸 x エッジ1
+	カプセル軸 x エッジ2
+	((カプセル点0-面点0) x エッジ0) x エッジ0
+	((カプセル点0-面点1) x エッジ1) x エッジ1
+	((カプセル点0-面点2) x エッジ2) x エッジ2
+	((カプセル点1-面点0) x エッジ0) x エッジ0
+	((カプセル点1-面点1) x エッジ1) x エッジ1
+	((カプセル点1-面点2) x エッジ2) x エッジ2
+	((面点0-カプセル点0) x カプセル軸) x カプセル軸
+	((面点1-カプセル点0) x カプセル軸) x カプセル軸
+	((面点2-カプセル点0) x カプセル軸) x カプセル軸
+	面点0-カプセル点0
+	面点1-カプセル点0
+	面点2-カプセル点0
+	面点0-カプセル点1
+	面点1-カプセル点1
+	面点2-カプセル点1
+ */
+
+static SCE_PFX_FORCE_INLINE
+bool checkSAT(const PfxVector3 &axis,PfxFloat AMin,PfxFloat AMax,PfxFloat BMin,PfxFloat BMax,PfxFloat &distMin,PfxVector3 &axisMin)
+{
+	// ■ 非接触.
+
+	// A:          +----+
+	// B:+----+
+	if(BMax <= AMin) {
+		return true;
+	}
+
+	// A:+----+
+	// B:          +----+
+	else if(AMax <= BMin) {
+		return true;
+	}
+
+	// ■ 内包
+
+	//A:      +--+
+	//B:   +------+
+	if(BMin < AMin && AMax < BMax) {
+		PfxFloat d = AMin-BMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = axis;
+		}
+	}
+
+	//A:   +------+
+	//B:    +--+
+	else if(AMin < BMin && BMax < AMax) {
+		PfxFloat d = AMin-BMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = axis;
+		}
+	}
+
+	// ■ 接触
+	
+	// A:   +----+
+	// B:+----+
+	else if(BMin < AMin && AMin < BMax) {
+		PfxFloat d = AMin-BMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = axis;
+		}
+	}
+
+	// A:   +----+
+	// B:      +----+
+	else if(AMin < BMin && BMin < AMax) {
+		PfxFloat d = BMin-AMax;
+		if(distMin < d) {
+			distMin = d;
+			axisMin = -axis;
+		}
+	}
+
+	return false;
+}
+
+static SCE_PFX_FORCE_INLINE
+bool pfxContactTriangleCapsule(PfxContactCache &contacts,PfxUInt32 facetId,
+							const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
+							const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
+							PfxUInt32 edgeChk,
+							PfxFloat capsuleRadius,const PfxVector3 &capP0,const PfxVector3 &capP1)
+{
+	const PfxFloat epsilon = 0.00001f;
+
+	// 最も浅い貫通深度とそのときの分離軸
+	PfxFloat distMin = -SCE_PFX_FLT_MAX;
+	PfxVector3 axisMin(0.0f);
+
+	//-------------------------------------------
+	// １．分離軸判定
+	{
+		PfxVector3 facetPnts[6] = {
+			p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
+		};
+
+		PfxVector3 sideNml[3] = {
+			normalize(cross((facetPnts[1] - facetPnts[0]),normal)),
+			normalize(cross((facetPnts[2] - facetPnts[1]),normal)),
+			normalize(cross((facetPnts[0] - facetPnts[2]),normal)),
+		};
+
+
+		const PfxVector3 capPnts[2] = {
+			capP0,capP1
+		};
+
+		{
+			const PfxVector3 &sepAxis = normal;
+
+			// 分離平面
+			PfxPlane plane(sepAxis,facetPnts[0]);
+
+			// Capsule(B)を分離軸に投影して範囲を取得
+			PfxFloat test1,test2,BMin,BMax;
+			test1 = plane.onPlane(capP0);
+			test2 = plane.onPlane(capP1);
+			BMax = SCE_PFX_MAX(test1,test2) + capsuleRadius;
+			BMin = SCE_PFX_MIN(test1,test2) - capsuleRadius;
+
+			// 判定
+			if(BMin > 0.0f || BMax < -thickness) {
+				return false;
+			}
+
+			if(distMin < BMin) {
+				distMin = BMin;
+				axisMin = -sepAxis;
+			}
+		}
+
+		//-------------------------------------------
+		// カプセル軸 x 面エッジ0,1,2
+		// ※Triangles座標系 (Aローカル)
+
+		{
+			for(int e=0;e<3;e++) {
+				PfxVector3 sepAxis = cross(capP1-capP0,facetPnts[(e+1)%3]-facetPnts[e]);
+				PfxFloat l=length(sepAxis);
+				if(l < 0.00001f) continue;
+				sepAxis /= l;
+				
+				if(dot(normal,sepAxis) > 0.0f)
+					sepAxis = -sepAxis;
+				
+				// Triangleを分離軸に投影
+				PfxFloat AMin=SCE_PFX_FLT_MAX,AMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts6(facetPnts,sepAxis,AMin,AMax);
+				
+				// カプセルを分離軸に投影
+				PfxFloat BMin=SCE_PFX_FLT_MAX,BMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts2(capPnts,sepAxis,BMin,BMax);
+				BMin -= capsuleRadius;
+				BMax += capsuleRadius;
+				
+				if(checkSAT(sepAxis,AMin,AMax,BMin,BMax,distMin,axisMin)) {
+					return false;
+				}
+			}
+		}
+
+		//-------------------------------------------
+		// ((カプセル点0-面点0,1,2) x エッジ0,1,2) x エッジ0,1,2
+		// ※Triangles座標系 (Aローカル)
+		
+		{
+			for(int e=0;e<3;e++) {
+				PfxVector3 edge = facetPnts[(e+1)%3]-facetPnts[e];
+				PfxVector3 sepAxis = normalize(cross(cross(capP0-facetPnts[e],edge),edge));
+				
+				if(dot(normal,sepAxis) > 0.0f)
+					sepAxis = -sepAxis;
+				
+				// Triangleを分離軸に投影
+				PfxFloat AMin=SCE_PFX_FLT_MAX,AMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts6(facetPnts,sepAxis,AMin,AMax);
+				
+				// カプセルを分離軸に投影
+				PfxFloat BMin=SCE_PFX_FLT_MAX,BMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts2(capPnts,sepAxis,BMin,BMax);
+				BMin -= capsuleRadius;
+				BMax += capsuleRadius;
+				
+				if(checkSAT(sepAxis,AMin,AMax,BMin,BMax,distMin,axisMin)) {
+					return false;
+				}
+			}
+		}
+
+		//-------------------------------------------
+		// ((カプセル点1-面点0,1,2) x エッジ0,1,2) x エッジ0,1,2
+		// ※Triangles座標系 (Aローカル)
+		
+		{
+			for(int e=0;e<3;e++) {
+				PfxVector3 edge = facetPnts[(e+1)%3]-facetPnts[e];
+				PfxVector3 sepAxis = normalize(cross(cross(capP1-facetPnts[e],edge),edge));
+				
+				if(dot(normal,sepAxis) > 0.0f)
+					sepAxis = -sepAxis;
+				
+				// Triangleを分離軸に投影
+				PfxFloat AMin=SCE_PFX_FLT_MAX,AMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts6(facetPnts,sepAxis,AMin,AMax);
+				
+				// カプセルを分離軸に投影
+				PfxFloat BMin=SCE_PFX_FLT_MAX,BMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts2(capPnts,sepAxis,BMin,BMax);
+				BMin -= capsuleRadius;
+				BMax += capsuleRadius;
+				
+				if(checkSAT(sepAxis,AMin,AMax,BMin,BMax,distMin,axisMin)) {
+					return false;
+				}
+			}
+		}
+
+		//-------------------------------------------
+		// ((面点0,1,2-カプセル点0) x カプセル軸) x カプセル軸
+		// ※Triangles座標系 (Aローカル)
+
+		{
+			for(int e=0;e<3;e++) {
+				PfxVector3 capdir = capP1-capP0;
+				PfxVector3 sepAxis = cross(cross(facetPnts[e]-capP0,capdir),capdir);
+				PfxFloat l=length(sepAxis);
+				if(l < 0.00001f) continue;
+				sepAxis /= l;
+
+				if(dot(normal,sepAxis) > 0.0f)
+					sepAxis = -sepAxis;
+
+				// Triangleを分離軸に投影
+				PfxFloat AMin=SCE_PFX_FLT_MAX,AMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts6(facetPnts,sepAxis,AMin,AMax);
+				
+				// カプセルを分離軸に投影
+				PfxFloat BMin=SCE_PFX_FLT_MAX,BMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts2(capPnts,sepAxis,BMin,BMax);
+				BMin -= capsuleRadius;
+				BMax += capsuleRadius;
+				
+				if(checkSAT(sepAxis,AMin,AMax,BMin,BMax,distMin,axisMin)) {
+					return false;
+				}
+			}
+		}
+
+		//-------------------------------------------
+		// 面点0,1,2-カプセル点0
+		// ※Triangles座標系 (Aローカル)
+
+		{
+			for(int e=0;e<3;e++) {
+				PfxVector3 sepAxis = normalize(facetPnts[e]-capP0);
+
+				// Triangleを分離軸に投影
+				PfxFloat AMin=SCE_PFX_FLT_MAX,AMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts6(facetPnts,sepAxis,AMin,AMax);
+				
+				// カプセルを分離軸に投影
+				PfxFloat BMin=SCE_PFX_FLT_MAX,BMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts2(capPnts,sepAxis,BMin,BMax);
+				BMin -= capsuleRadius;
+				BMax += capsuleRadius;
+				
+				if(checkSAT(sepAxis,AMin,AMax,BMin,BMax,distMin,axisMin)) {
+					return false;
+				}
+			}
+		}
+
+		//-------------------------------------------
+		// 面点0,1,2-カプセル点1
+		// ※Triangles座標系 (Aローカル)
+
+		{
+			for(int e=0;e<3;e++) {
+				PfxVector3 sepAxis = normalize(facetPnts[e]-capP1);
+				
+				// Triangleを分離軸に投影
+				PfxFloat AMin=SCE_PFX_FLT_MAX,AMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts6(facetPnts,sepAxis,AMin,AMax);
+				
+				// カプセルを分離軸に投影
+				PfxFloat BMin=SCE_PFX_FLT_MAX,BMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts2(capPnts,sepAxis,BMin,BMax);
+				BMin -= capsuleRadius;
+				BMax += capsuleRadius;
+				
+				if(checkSAT(sepAxis,AMin,AMax,BMin,BMax,distMin,axisMin)) {
+					return false;
+				}
+			}
+		}
+
+		// 面に厚みがある場合の補助的な判定（面法線 x カプセル軸）
+		// 交差するかしないかだけを判定
+		if(thickness > 0.0f) {
+			// 厚み側面の法線
+			for(int i=0;i<3;i++) {
+				// 分離平面
+				PfxPlane plane(sideNml[i],facetPnts[i]);
+
+				// カプセルを分離軸に投影して範囲を取得
+				PfxFloat test1,test2,BMin;
+				test1 = plane.onPlane(capP0);
+				test2 = plane.onPlane(capP1);
+				BMin = SCE_PFX_MIN(test1,test2) - capsuleRadius;
+
+				if(BMin > 0.0f) {
+					return false;
+				}
+			}
+
+			// ２つの厚み側面のなすエッジ3×カプセル軸
+			for(int e=0;e<3;e++) {
+				PfxVector3 edgeVec = normalize(cross(sideNml[(e+1)%3],sideNml[e]));
+				PfxVector3 capVec = capP1-capP0;
+
+				// エッジが平行であれば判定しない
+				if(pfxIsSameDirection(edgeVec,capVec)) continue;
+
+				PfxVector3 sepAxis = normalize(cross(edgeVec,capVec));
+
+				// Trianglesを分離軸に投影して範囲を取得
+				PfxFloat triMin,triMax;
+				pfxGetProjAxisPnts3(facetPnts,sepAxis,triMin,triMax);
+
+				// カプセルを分離軸に投影して範囲を取得
+				PfxFloat BMin=SCE_PFX_FLT_MAX,BMax=-SCE_PFX_FLT_MAX;
+				pfxGetProjAxisPnts2(capPnts,sepAxis,BMin,BMax);
+				BMin -= capsuleRadius;
+				BMax += capsuleRadius;
+				
+				if(triMax < BMin || BMax < triMin) {
+					return false;
+				}
+			}
+		}
+	}
+	
+	//-------------------------------------------
+	// ２．衝突点の探索
+	
+	{
+		// 分離軸方向に引き離す(最近接を判定するため、交差回避させる)
+		PfxVector3 sepAxis = 1.1f * fabsf(distMin) * axisMin;
+		
+		const PfxVector3 facetPnts[3] = {
+			p0 + sepAxis,
+			p1 + sepAxis,
+			p2 + sepAxis,
+		};
+
+		//--------------------------------------------------------------------
+		// 衝突点の探索
+
+		PfxClosestPoints cp;
+		PfxVector3 sA,sB;
+
+		//--------------------------------------------------------------------
+		//Triangleの頂点 -> Capsule
+
+		// カプセルの線分と面のエッジx3の最近接点の算出
+#ifdef SCE_PFX_USE_GEOMETRY
+		PfxGeomSegment segmentC((PfxPoint3)capP0,(PfxPoint3)capP1);
+
+		{
+			PfxGeomSegment segmentF((PfxPoint3)facetPnts[0],(PfxPoint3)facetPnts[1]);
+			PfxPoint3 closestPointA,closestPointB;
+			PfxFloatInVec sqrDist = closestPoints(segmentC,segmentF,closestPointB,closestPointA);
+			cp.add(closestPointA,closestPointB + normalize(closestPointA-closestPointB)*capsuleRadius,sqrDist);
+		}
+		{
+			PfxGeomSegment segmentF((PfxPoint3)facetPnts[1],(PfxPoint3)facetPnts[2]);
+			PfxPoint3 closestPointA,closestPointB;
+			PfxFloatInVec sqrDist = closestPoints(segmentC,segmentF,closestPointB,closestPointA);
+			cp.add(closestPointA,closestPointB + normalize(closestPointA-closestPointB)*capsuleRadius,sqrDist);
+		}
+		{
+			PfxGeomSegment segmentF((PfxPoint3)facetPnts[2],(PfxPoint3)facetPnts[0]);
+			PfxPoint3 closestPointA,closestPointB;
+			PfxFloatInVec sqrDist = closestPoints(segmentC,segmentF,closestPointB,closestPointA);
+			cp.add(closestPointA,closestPointB + normalize(closestPointA-closestPointB)*capsuleRadius,sqrDist);
+		}
+#else
+		pfxClosestTwoLines(capP0,capP1,facetPnts[0],facetPnts[1],sB,sA);
+		cp.add(PfxPoint3(sA),PfxPoint3(sB + normalize(sA-sB)*capsuleRadius),lengthSqr(sA-sB));
+		
+		pfxClosestTwoLines(capP0,capP1,facetPnts[1],facetPnts[2],sB,sA);
+		cp.add(PfxPoint3(sA),PfxPoint3(sB + normalize(sA-sB)*capsuleRadius),lengthSqr(sA-sB));
+
+		pfxClosestTwoLines(capP0,capP1,facetPnts[2],facetPnts[0],sB,sA);
+		cp.add(PfxPoint3(sA),PfxPoint3(sB + normalize(sA-sB)*capsuleRadius),lengthSqr(sA-sB));
+#endif
+
+		// カプセルの端点と面の最近接点の算出
+		PfxTriangle triangleA(facetPnts[0],facetPnts[1],facetPnts[2]);
+		pfxClosestPointTriangle(capP0,triangleA,sA);
+		cp.add(PfxPoint3(sA),PfxPoint3(capP0+normalize(sA-capP0)*capsuleRadius),lengthSqr(sA-capP0));
+
+		pfxClosestPointTriangle(capP1,triangleA,sA);
+		cp.add(PfxPoint3(sA),PfxPoint3(capP1+normalize(sA-capP1)*capsuleRadius),lengthSqr(sA-capP1));
+
+		for(int i=0;i<cp.numPoints;i++) {
+			if(cp.distSqr[i] < cp.closestDistSqr + epsilon) {
+				cp.pA[i] -= sepAxis;
+				// 面上の最近接点が凸エッジ上でない場合は法線を変える
+				if( ((edgeChk&0x01)&&pfxPointOnLine(PfxVector3(cp.pA[i]),p0,p1)) ||
+					((edgeChk&0x02)&&pfxPointOnLine(PfxVector3(cp.pA[i]),p1,p2)) ||
+					((edgeChk&0x04)&&pfxPointOnLine(PfxVector3(cp.pA[i]),p2,p0)) ) {
+					axisMin=-normal;
+				}
+
+				PfxSubData subData;
+				subData.setFacetId(facetId);
+				contacts.addContactPoint(-length(cp.pB[i]-cp.pA[i]),axisMin,cp.pA[i],cp.pB[i],subData);
+			}
+		}
+	}
+
+	return true;
+}
+
+PfxInt32 pfxContactTriMeshCapsule(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxCapsule &capsuleB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	(void) distanceThreshold;
+
+	PfxTransform3 transformAB, transformBA;
+	PfxMatrix3 matrixBA;
+	PfxVector3 offsetBA;
+
+	// Bローカル→Aローカルへの変換.
+	transformAB = orthoInverse(transformA) * transformB;
+
+	// Aローカル→Bローカルへの変換.
+	transformBA = orthoInverse(transformAB);
+
+	matrixBA = transformBA.getUpper3x3();
+	offsetBA = transformBA.getTranslation();
+
+	//-------------------------------------------
+	// 判定する面を絞り込む.
+
+	PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = { 0 };
+	PfxUInt32 numSelFacets = 0;
+
+	// ※capsuleB座標系
+	PfxVector3 aabbB(capsuleB.m_halfLen+capsuleB.m_radius,capsuleB.m_radius,capsuleB.m_radius);
+	numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
+
+	if(numSelFacets == 0) {
+		return 0;
+	}
+	
+	//-------------------------------------------
+	// 分離軸判定(SAT)
+	// ※CapsuleB座標系 (Bローカル)で判定
+
+	PfxVector3 vCapAB[2] = {
+		PfxVector3(-capsuleB.m_halfLen, 0.0f, 0.0f),
+		PfxVector3( capsuleB.m_halfLen, 0.0f, 0.0f)
+	};
+	
+	PfxContactCache localContacts;
+	
+	for(PfxUInt32 f = 0; f < numSelFacets; f++) {
+		const PfxFacet &facet = meshA->m_facets[selFacets[f]];
+		
+		PfxVector3 facetNormal = matrixBA * pfxReadVector3(facet.m_normal);
+
+		PfxVector3 facetPntsA[3] = {
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[0]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[1]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[2]],
+		};
+
+		const PfxEdge *edge[3] = {
+			&meshA->m_edges[facet.m_edgeIds[0]],
+			&meshA->m_edges[facet.m_edgeIds[1]],
+			&meshA->m_edges[facet.m_edgeIds[2]],
+		};
+		
+		PfxUInt32 edgeChk = 
+			((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |
+			((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |
+			((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);
+
+		pfxContactTriangleCapsule(localContacts,selFacets[f],
+							facetNormal,facetPntsA[0],facetPntsA[1],facetPntsA[2],
+							facet.m_thickness,
+							0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),
+							edgeChk,capsuleB.m_radius,vCapAB[0],vCapAB[1]);
+	}
+	
+	for(int i=0;i<localContacts.getNumContacts();i++) {
+		PfxSubData subData = localContacts.getSubData(i);
+		
+		const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
+		
+		PfxTriangle triangleA(
+			meshA->m_verts[facet.m_vertIds[0]],
+			meshA->m_verts[facet.m_vertIds[1]],
+			meshA->m_verts[facet.m_vertIds[2]]);
+		
+		PfxFloat s=0.0f,t=0.0f;
+		pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);
+		subData.m_type = PfxSubData::MESH_INFO;
+		subData.setFacetLocalS(s);
+		subData.setFacetLocalT(t);
+		
+		contacts.addContactPoint(
+			localContacts.getDistance(i),
+			transformB.getUpper3x3() * localContacts.getNormal(i),
+			transformAB * localContacts.getLocalPointA(i),
+			localContacts.getLocalPointB(i),
+			subData);
+	}
+	
+	return contacts.getNumContacts();
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_capsule.h

@@ -0,0 +1,39 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_TRI_MESH_CAPSULE_H
+#define _SCE_PFX_CONTACT_TRI_MESH_CAPSULE_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_capsule.h"
+#include "pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxContactTriMeshCapsule(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxCapsule &capsuleB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX );
+
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_TRI_MESH_CAPSULE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_convex.cpp

@@ -0,0 +1,167 @@
+/*
+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 "pfx_contact_tri_mesh_convex.h"
+#include "pfx_intersect_common.h"
+#include "pfx_mesh_common.h"
+#include "pfx_gjk_solver.h"
+#include "pfx_gjk_support_func.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+static SCE_PFX_FORCE_INLINE
+bool pfxContactTriangleConvex(PfxContactCache &contacts,PfxUInt32 facetId,
+							const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
+							const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
+							PfxUInt32 edgeChk,
+							const PfxConvexMesh &convex)
+{
+	PfxVector3 facetPnts[6] = {
+		p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
+	};
+	
+
+	PfxPoint3 pA(0.0f),pB(0.0f);
+	PfxVector3 nml(0.0f);
+	PfxGjkSolver gjk;
+
+	gjk.setup((void*)facetPnts,(void*)&convex,pfxGetSupportVertexTriangleWithThickness,pfxGetSupportVertexConvex);
+	PfxFloat d = gjk.collide(nml,pA,pB,PfxTransform3::identity(),PfxTransform3::identity(),SCE_PFX_FLT_MAX);
+	if(d >= 0.0f) return false;
+	
+	PfxVector3 pointsOnTriangle = PfxVector3(pA);
+	PfxVector3 pointsOnConvex = PfxVector3(pB);
+	PfxVector3 axis = nml;
+	
+	// 面上の最近接点が凸エッジ上でない場合は法線を変える
+	if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
+		((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
+		((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
+		axis=-normal;
+	}
+	
+	PfxSubData subData;
+	subData.setFacetId(facetId);
+	contacts.addContactPoint(-length(pointsOnTriangle-pointsOnConvex),axis,pA,pB,subData);
+	
+	return true;
+}
+
+PfxInt32 pfxContactTriMeshConvex(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxConvexMesh &convexB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	(void) distanceThreshold;
+
+	PfxTransform3 transformAB, transformBA;
+	PfxMatrix3 matrixBA;
+	PfxVector3 offsetBA;
+
+	// Bローカル→Aローカルへの変換.
+	transformAB = orthoInverse(transformA) * transformB;
+
+	// Aローカル→Bローカルへの変換.
+	transformBA = orthoInverse(transformAB);
+
+	matrixBA = transformBA.getUpper3x3();
+	offsetBA = transformBA.getTranslation();
+
+	//-------------------------------------------
+	// 判定する面を絞り込む.
+
+	PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = { 0 };
+	PfxUInt32 numSelFacets = 0;
+
+	// ※convexB座標系
+	PfxVector3 aabbB(convexB.m_half);
+	numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
+
+	if(numSelFacets == 0) {
+		return 0;
+	}
+	
+	//-------------------------------------------
+	// 面ごとに衝突を検出
+	
+	PfxContactCache localContacts;
+	
+	for(PfxUInt32 f = 0; f < numSelFacets; f++) {
+		const PfxFacet &facet = meshA->m_facets[selFacets[f]];
+		
+		PfxVector3 facetNormal = matrixBA * pfxReadVector3(facet.m_normal);
+
+		PfxVector3 facetPntsA[3] = {
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[0]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[1]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[2]],
+		};
+
+		const PfxEdge *edge[3] = {
+			&meshA->m_edges[facet.m_edgeIds[0]],
+			&meshA->m_edges[facet.m_edgeIds[1]],
+			&meshA->m_edges[facet.m_edgeIds[2]],
+		};
+		
+		PfxUInt32 edgeChk = 
+			((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |
+			((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |
+			((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);
+
+		pfxContactTriangleConvex(localContacts,selFacets[f],
+							facetNormal,facetPntsA[0],facetPntsA[1],facetPntsA[2],
+							facet.m_thickness,
+							0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),
+							edgeChk,convexB);
+	}
+	
+	for(int i=0;i<localContacts.getNumContacts();i++) {
+		PfxSubData subData = localContacts.getSubData(i);
+		
+		const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
+		
+		PfxTriangle triangleA(
+			meshA->m_verts[facet.m_vertIds[0]],
+			meshA->m_verts[facet.m_vertIds[1]],
+			meshA->m_verts[facet.m_vertIds[2]]);
+		
+		PfxFloat s=0.0f,t=0.0f;
+		pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);
+		subData.m_type = PfxSubData::MESH_INFO;
+		subData.setFacetLocalS(s);
+		subData.setFacetLocalT(t);
+		
+		contacts.addContactPoint(
+			localContacts.getDistance(i),
+			transformB.getUpper3x3() * localContacts.getNormal(i),
+			transformAB * localContacts.getLocalPointA(i),
+			localContacts.getLocalPointB(i),
+			subData);
+	}
+	
+	return contacts.getNumContacts();
+}
+} //namespace PhysicsEffects
+} //namespace sce
+

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_convex.h

@@ -0,0 +1,37 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_TRI_MESH_CONVEX_H
+#define _SCE_PFX_CONTACT_TRI_MESH_CONVEX_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"
+#include "pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxContactTriMeshConvex(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxConvexMesh &convexB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX );
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_TRI_MESH_CONVEX_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_cylinder.cpp

@@ -0,0 +1,165 @@
+/*
+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 "pfx_contact_tri_mesh_cylinder.h"
+#include "pfx_intersect_common.h"
+#include "pfx_mesh_common.h"
+#include "pfx_gjk_solver.h"
+#include "pfx_gjk_support_func.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+static SCE_PFX_FORCE_INLINE
+bool pfxContactTriangleCylinder(PfxContactCache &contacts,PfxUInt32 facetId,
+							const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
+							const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
+							PfxUInt32 edgeChk,
+							const PfxCylinder &cylinder)
+{
+	PfxVector3 facetPnts[6] = {
+		p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
+	};
+	
+
+	PfxPoint3 pA(0.0f),pB(0.0f);
+	PfxVector3 nml(0.0f);
+	PfxGjkSolver gjk;
+	
+	gjk.setup((void*)facetPnts,(void*)&cylinder,pfxGetSupportVertexTriangleWithThickness,pfxGetSupportVertexCylinder);
+	PfxFloat d = gjk.collide(nml,pA,pB,PfxTransform3::identity(),PfxTransform3::identity(),SCE_PFX_FLT_MAX);
+	if(d >= 0.0f) return false;
+	
+	PfxVector3 pointsOnTriangle = PfxVector3(pA);
+	PfxVector3 pointsOnConvex = PfxVector3(pB);
+	PfxVector3 axis = nml;
+	
+	// 面上の最近接点が凸エッジ上でない場合は法線を変える
+	if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
+		((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
+		((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
+		axis=-normal;
+	}
+	
+	PfxSubData subData;
+	subData.setFacetId(facetId);
+	contacts.addContactPoint(-length(pointsOnTriangle-pointsOnConvex),axis,pA,pB,subData);
+	
+	return true;
+}
+
+PfxInt32 pfxContactTriMeshCylinder(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxCylinder &cylinderB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	(void) distanceThreshold;
+
+	PfxTransform3 transformAB, transformBA;
+	PfxMatrix3 matrixBA;
+	PfxVector3 offsetBA;
+
+	// Bローカル→Aローカルへの変換.
+	transformAB = orthoInverse(transformA) * transformB;
+
+	// Aローカル→Bローカルへの変換.
+	transformBA = orthoInverse(transformAB);
+
+	matrixBA = transformBA.getUpper3x3();
+	offsetBA = transformBA.getTranslation();
+
+	//-------------------------------------------
+	// 判定する面を絞り込む.
+
+	PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = { 0 };
+	PfxUInt32 numSelFacets = 0;
+
+	// ※cylinderB座標系
+	PfxVector3 aabbB(cylinderB.m_halfLen,cylinderB.m_radius,cylinderB.m_radius);
+	numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
+
+	if(numSelFacets == 0) {
+		return 0;
+	}
+	
+	//-------------------------------------------
+	// 面ごとに衝突を検出
+	
+	PfxContactCache localContacts;
+	
+	for(PfxUInt32 f = 0; f < numSelFacets; f++) {
+		const PfxFacet &facet = meshA->m_facets[selFacets[f]];
+		
+		PfxVector3 facetNormal = matrixBA * pfxReadVector3(facet.m_normal);
+
+		PfxVector3 facetPntsA[3] = {
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[0]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[1]],
+			offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[2]],
+		};
+
+		const PfxEdge *edge[3] = {
+			&meshA->m_edges[facet.m_edgeIds[0]],
+			&meshA->m_edges[facet.m_edgeIds[1]],
+			&meshA->m_edges[facet.m_edgeIds[2]],
+		};
+		
+		PfxUInt32 edgeChk = 
+			((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |
+			((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |
+			((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);
+
+		pfxContactTriangleCylinder(localContacts,selFacets[f],
+							facetNormal,facetPntsA[0],facetPntsA[1],facetPntsA[2],
+							facet.m_thickness,
+							0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),
+							0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),
+							edgeChk,cylinderB);
+	}
+	
+	for(int i=0;i<localContacts.getNumContacts();i++) {
+		PfxSubData subData = localContacts.getSubData(i);
+		
+		const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
+		
+		PfxTriangle triangleA(
+			meshA->m_verts[facet.m_vertIds[0]],
+			meshA->m_verts[facet.m_vertIds[1]],
+			meshA->m_verts[facet.m_vertIds[2]]);
+		
+		PfxFloat s=0.0f,t=0.0f;
+		pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);
+		subData.m_type = PfxSubData::MESH_INFO;
+		subData.setFacetLocalS(s);
+		subData.setFacetLocalT(t);
+		
+		contacts.addContactPoint(
+			localContacts.getDistance(i),
+			transformB.getUpper3x3() * localContacts.getNormal(i),
+			transformAB * localContacts.getLocalPointA(i),
+			localContacts.getLocalPointB(i),
+			subData);
+	}
+	
+	return contacts.getNumContacts();
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_cylinder.h

@@ -0,0 +1,38 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_TRI_MESH_CYLINDER_H
+#define _SCE_PFX_CONTACT_TRI_MESH_CYLINDER_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_cylinder.h"
+#include "pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxContactTriMeshCylinder(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxCylinder &cylinderB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX );
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_TRI_MESH_CYLINDER_H

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_sphere.cpp

@@ -0,0 +1,239 @@
+/*
+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 "pfx_contact_tri_mesh_sphere.h"
+#include "pfx_intersect_common.h"
+#include "pfx_mesh_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+static SCE_PFX_FORCE_INLINE
+bool pfxContactTriangleSphere(PfxContactCache &contacts,PfxUInt32 facetId,
+	const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
+	const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
+	PfxUInt32 edgeChk,
+	PfxFloat sphereRadius,const PfxVector3 &spherePos)
+{
+	PfxVector3 facetPnts[3] = {
+		p0,p1,p2,
+	};
+	
+	// 早期判定
+	{
+		PfxPlane planeA(normal,p0);
+		PfxFloat len1 = planeA.onPlane(spherePos);
+		
+		if(len1 >= sphereRadius || len1 < -thickness-sphereRadius) return false;
+		
+	}
+
+	// 球と面の最近接点を計算
+	{
+		PfxTriangle triangleA(p0,p1,p2);
+		PfxVector3 pntA;
+		// pfxClosestPointTriangle(spherePos,triangleA,pntA);
+		bool insideTriangle = false;
+		while(1) {
+		    PfxVector3 ab = p1 - p0;
+		    PfxVector3 ac = p2 - p0;
+		    PfxVector3 ap = spherePos - p0;
+		    PfxFloat d1 = dot(ab, ap);
+		    PfxFloat d2 = dot(ac, ap);
+			if(d1 <= 0.0f && d2 <= 0.0f) {
+				pntA = p0;
+				break;
+			}
+
+		    PfxVector3 bp = spherePos - p1;
+		    PfxFloat d3 = dot(ab, bp);
+		    PfxFloat d4 = dot(ac, bp);
+			if (d3 >= 0.0f && d4 <= d3) {
+				pntA = p1;
+				break;
+			}
+
+		    PfxFloat vc = d1*d4 - d3*d2;
+		    if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
+		        PfxFloat v = d1 / (d1 - d3);
+		        pntA = p0 + v * ab;
+				break;
+		    }
+
+		    PfxVector3 cp = spherePos - p2;
+		    PfxFloat d5 = dot(ab, cp);
+		    PfxFloat d6 = dot(ac, cp);
+			if (d6 >= 0.0f && d5 <= d6) {
+				pntA = p2;
+				break;
+			}
+
+		    PfxFloat vb = d5*d2 - d1*d6;
+		    if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
+		        PfxFloat w = d2 / (d2 - d6);
+		        pntA = p0 + w * ac;
+				break;
+		    }
+
+		    PfxFloat va = d3*d6 - d5*d4;
+		    if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
+		        PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		        pntA = p1 + w * (p2 - p1);
+				break;
+		    }
+
+		    PfxFloat den = 1.0f / (va + vb + vc);
+		    PfxFloat v = vb * den;
+		    PfxFloat w = vc * den;
+		    pntA = p0 + ab * v + ac * w;
+		    insideTriangle = true;
+			break;
+		}
+		PfxVector3 distVec = pntA - spherePos;
+		PfxFloat l = length(distVec);
+		
+		if(!insideTriangle && l >= sphereRadius) return false;
+		
+		// 分離軸
+		PfxVector3 sepAxis = (l < 0.00001f || insideTriangle) ? -normal : distVec / l;
+
+		// 球上の衝突点
+		PfxVector3 pointsOnSphere = spherePos + sphereRadius * sepAxis;
+		PfxVector3 pointsOnTriangle = pntA;
+
+		// 面上の最近接点が凸エッジ上でない場合は法線を変える
+		if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
+			((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
+			((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
+			sepAxis=-normal;
+		}
+
+		PfxSubData subData;
+		subData.setFacetId(facetId);
+		contacts.addContactPoint(-length(pointsOnSphere-pointsOnTriangle),sepAxis,PfxPoint3(pointsOnTriangle),PfxPoint3(pointsOnSphere),subData);
+	}
+
+	return true;
+}
+
+PfxInt32 pfxContactTriMeshSphere(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxSphere &sphereB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold)
+{
+	(void) distanceThreshold;
+
+	PfxTransform3 transformAB,transformBA;
+	PfxMatrix3 matrixBA;
+	PfxVector3 offsetBA;
+
+	// Bローカル→Aローカルへの変換
+	transformAB = orthoInverse(transformA) * transformB;
+
+	// Aローカル→Bローカルへの変換
+	transformBA = orthoInverse(transformAB);
+
+	matrixBA = transformBA.getUpper3x3();
+	offsetBA = transformBA.getTranslation();
+
+	//-------------------------------------------
+	// 判定する面を絞り込む
+
+	PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = {0};
+	PfxUInt32 numSelFacets = 0;
+
+	PfxVector3 aabbB(sphereB.m_radius);
+	numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
+
+	if(numSelFacets == 0) {
+		return 0;
+	}
+
+	//-----------------------------------------------
+	// 判定
+
+	PfxContactCache localContacts;
+
+	// TriangleMeshの面->sphereの判定
+	// ※TriangleMesh座標系
+	{
+		for(PfxUInt32 f = 0; f < numSelFacets; f++ ) {
+			const PfxFacet &facet = meshA->m_facets[selFacets[f]];
+
+			const PfxVector3 facetNormal = pfxReadVector3(facet.m_normal);
+
+			const PfxVector3 facetPnts[3] = {
+				meshA->m_verts[facet.m_vertIds[0]],
+				meshA->m_verts[facet.m_vertIds[1]],
+				meshA->m_verts[facet.m_vertIds[2]],
+			};
+			
+			const PfxEdge *edge[3] = {
+				&meshA->m_edges[facet.m_edgeIds[0]],
+				&meshA->m_edges[facet.m_edgeIds[1]],
+				&meshA->m_edges[facet.m_edgeIds[2]],
+			};
+			
+			PfxVector3 sepAxis,pntA,pntB;
+			
+			PfxUInt32 edgeChk = 
+				((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |
+				((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |
+				((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);
+			
+			pfxContactTriangleSphere(localContacts,selFacets[f],
+									facetNormal,facetPnts[0],facetPnts[1],facetPnts[2],
+									facet.m_thickness,
+									0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),
+									0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),
+									0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),
+									edgeChk,
+									sphereB.m_radius,transformAB.getTranslation());
+		}
+	}
+
+	for(int i=0;i<localContacts.getNumContacts();i++) {
+		PfxSubData subData = localContacts.getSubData(i);
+		
+		const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
+		
+		PfxTriangle triangleA(
+			meshA->m_verts[facet.m_vertIds[0]],
+			meshA->m_verts[facet.m_vertIds[1]],
+			meshA->m_verts[facet.m_vertIds[2]]);
+		
+		PfxFloat s=0.0f,t=0.0f;
+		pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);
+		subData.m_type = PfxSubData::MESH_INFO;
+		subData.setFacetLocalS(s);
+		subData.setFacetLocalT(t);
+		
+		contacts.addContactPoint(
+			localContacts.getDistance(i),
+			transformA.getUpper3x3() * localContacts.getNormal(i),
+			localContacts.getLocalPointA(i),
+			transformBA * localContacts.getLocalPointB(i),
+			subData);
+	}
+
+	return contacts.getNumContacts();
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_sphere.h

@@ -0,0 +1,38 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONTACT_TRI_MESH_SPHERE_H
+#define _SCE_PFX_CONTACT_TRI_MESH_SPHERE_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_sphere.h"
+#include "pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxContactTriMeshSphere(
+	PfxContactCache &contacts,
+	const PfxTriMesh *meshA,
+	const PfxTransform3 &transformA,
+	const PfxSphere &sphereB,
+	const PfxTransform3 &transformB,
+	PfxFloat distanceThreshold = SCE_PFX_FLT_MAX );
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_TRI_MESH_SPHERE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_gjk_solver.cpp

@@ -0,0 +1,485 @@
+/*
+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 "pfx_gjk_solver.h"
+#include "pfx_intersect_common.h"
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+template <class T, int maxData = 20>
+struct PfxGjkStack {
+	int cur;
+	T data[maxData];
+
+	PfxGjkStack() : cur(0) {}
+
+	void push(const T &d)
+	{
+		SCE_PFX_ASSERT(cur<maxData-1);
+		data[cur++] = d;
+	}
+	
+	T pop()
+	{
+		return data[--cur];
+	}
+
+	bool isEmpty()
+	{
+		return cur == 0;
+	}
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Allocate Buffers
+
+PfxGjkSolver::PfxGjkSolver()
+{
+vertsP = g_vertsP;
+vertsQ = g_vertsQ;
+vertsW = g_vertsW;
+facets = g_facets;
+facetsHead = g_facetsHead;
+edges = g_edges;
+}
+
+PfxGjkSolver::~PfxGjkSolver()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Setup Buffers
+
+void PfxGjkSolver::setup(void *sA,void *sB,PfxGetSupportVertexFunc fA,PfxGetSupportVertexFunc fB)
+{
+	shapeA = sA;
+	shapeB = sB;
+	getSupportVertexShapeA = fA;
+	getSupportVertexShapeB = fB;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Construct Silhouette
+
+void PfxGjkSolver::silhouette(PfxGjkFacet *facet,int i,PfxVector3 &w)
+{
+	PfxGjkStack<PfxGjkEdge> gs;
+
+	gs.push(PfxGjkEdge(facet,i));
+
+	do {
+		PfxGjkEdge stk = gs.pop();
+		PfxGjkFacet *ft = stk.f;
+
+		if(ft->obsolete==0) {
+			// wから見えるかどうかを判定
+			if(dot(ft->normal,w-ft->closest) < 0.0f) {
+				// 見えないのでエッジを登録
+				SCE_PFX_ASSERT(numEdges<=(int)MAX_FACETS);
+				edges[numEdges] = stk;
+				numEdges++;
+			}
+			else {
+				ft->obsolete = 1;
+				gs.push(PfxGjkEdge(ft->adj[(stk.i+2)%3],ft->j[(stk.i+2)%3]));
+				gs.push(PfxGjkEdge(ft->adj[(stk.i+1)%3],ft->j[(stk.i+1)%3]));
+			}
+		}
+	} while(SCE_PFX_UNLIKELY(!gs.isEmpty()));
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Detect Penetration Depth (EPA)
+
+PfxFloat PfxGjkSolver::detectPenetrationDepth(
+	const PfxTransform3 &transformA,const PfxTransform3 &transformB,
+	PfxVector3 &pA,PfxVector3 &pB,PfxVector3 &normal)
+{
+	PfxMatrix3 invRotA = transpose(transformA.getUpper3x3());
+	PfxMatrix3 invRotB = transpose(transformB.getUpper3x3());
+
+	int epaIterationCount = 0;
+	PfxFloat distance = SCE_PFX_FLT_MAX;
+
+	numFacets = 0;
+	numFacetsHead = 0;
+
+	// 初期状態の判定
+	if(m_simplex.numVertices <= 1) {
+		return distance;
+	}
+	else if(m_simplex.numVertices == 2) {
+		PfxVector3 v0 = m_simplex.W[0];
+		PfxVector3 v1 = m_simplex.W[1];
+		PfxVector3 dir = normalize(v1-v0);
+		PfxMatrix3 rot = PfxMatrix3::rotation(2.0943951023932f,dir);//120 deg
+		int axis;
+		if(dir[0] < dir[1]) {
+			if(dir[0] < dir[2]) {
+				axis = 0;
+			}
+			else {
+				axis = 2;
+			}
+		}
+		else {
+			if(dir[1] < dir[2]) {
+				axis = 1;
+			}
+			else {
+				axis = 2;
+			}
+		}
+        PfxVector3 vec(0.0f);
+		vec[axis] = 1.0f;
+
+		PfxVector3 aux[3];
+		aux[0] = cross(dir,vec);
+		aux[1] = rot * aux[0];
+		aux[2] = rot * aux[1];
+
+		PfxVector3 p[3],q[3],w[3];
+
+		for(int i=0;i<3;i++) {
+			PfxVector3 pInA,qInB;
+			getSupportVertexShapeA(shapeA,invRotA * aux[i],pInA);
+			getSupportVertexShapeB(shapeB,invRotB * (-aux[i]),qInB);
+			p[i] = transformA.getTranslation() + transformA.getUpper3x3() * pInA;
+			q[i] = transformB.getTranslation() + transformB.getUpper3x3() * qInB;
+			w[i] = p[i] - q[i];
+			vertsP[i] = p[i];
+			vertsQ[i] = q[i];
+			vertsW[i] = w[i];
+		}
+
+		if(originInTetrahedron(w[0],w[1],w[2],v0)) {
+			vertsP[3] = m_simplex.P[0];
+			vertsQ[3] = m_simplex.Q[0];
+			vertsW[3] = m_simplex.W[0];
+			numVerts = 4;
+		}
+		else if(originInTetrahedron(w[0],w[1],w[2],v1)){
+			vertsP[3] = m_simplex.P[1];
+			vertsQ[3] = m_simplex.Q[1];
+			vertsW[3] = m_simplex.W[1];
+			numVerts = 4;
+		}
+		else {
+			return distance;
+		}
+	}
+	else if(m_simplex.numVertices == 3) {
+		numVerts = 3;
+		for(int i=0;i<numVerts;i++) {
+			vertsP[i] = m_simplex.P[i];
+			vertsQ[i] = m_simplex.Q[i];
+			vertsW[i] = m_simplex.W[i];
+		}
+
+		PfxVector3 p[2],q[2],w[2];
+
+		{
+			PfxVector3 v = cross(vertsW[2]-vertsW[0],vertsW[1]-vertsW[0]);
+			PfxVector3 pInA,qInB;
+			getSupportVertexShapeA(shapeA,invRotA * v,pInA);
+			getSupportVertexShapeB(shapeB,invRotB * (-v),qInB);
+			p[0] = transformA.getTranslation() + transformA.getUpper3x3() * pInA;
+			q[0] = transformB.getTranslation() + transformB.getUpper3x3() * qInB;
+			w[0] = p[0] - q[0];
+			getSupportVertexShapeA(shapeA,invRotA * (-v),pInA);
+			getSupportVertexShapeB(shapeB,invRotB * v,qInB);
+			p[1] = transformA.getTranslation() + transformA.getUpper3x3() * pInA;
+			q[1] = transformB.getTranslation() + transformB.getUpper3x3() * qInB;
+			w[1] = p[1] - q[1];
+		}
+
+		if(originInTetrahedron(vertsW[0],vertsW[1],vertsW[2],w[0])) {
+			vertsP[3] = p[0];
+			vertsQ[3] = q[0];
+			vertsW[3] = w[0];
+			numVerts = 4;
+		}
+		else if(originInTetrahedron(vertsW[0],vertsW[1],vertsW[2],w[1])){
+			vertsP[3] = p[1];
+			vertsQ[3] = q[1];
+			vertsW[3] = w[1];
+			numVerts = 4;
+		}
+		else {
+			return distance;
+		}
+	}
+	else {
+		numVerts = 4;
+		for(int i=0;i<numVerts;i++) {
+			vertsP[i] = m_simplex.P[i];
+			vertsQ[i] = m_simplex.Q[i];
+			vertsW[i] = m_simplex.W[i];
+		}
+	}
+
+	SCE_PFX_ASSERT(numVerts == 4);
+
+	// 原点が単体の内部にあるかどうかを判定
+	if(SCE_PFX_UNLIKELY(!originInTetrahedron(vertsW[0],vertsW[1],vertsW[2],vertsW[3]))) {
+		return distance;
+	}
+
+	// 面の向きをチェック
+	if(dot(-vertsW[0],cross(vertsW[2]-vertsW[0],vertsW[1]-vertsW[0])) > 0.0f) {
+		PfxVector3 vertsP1,vertsQ1,vertsW1;
+		PfxVector3 vertsP3,vertsQ3,vertsW3;
+		vertsQ1=vertsQ[1];vertsW1=vertsW[1];vertsP1=vertsP[1];
+		vertsQ3=vertsQ[3];vertsW3=vertsW[3];vertsP3=vertsP[3];
+		vertsQ[1]=vertsQ3;vertsW[1]=vertsW3;vertsP[1]=vertsP3;
+		vertsQ[3]=vertsQ1;vertsW[3]=vertsW1;vertsP[3]=vertsP1;
+	}
+
+	{
+		PfxGjkFacet *f0 = addFacet(0,1,2);
+		PfxGjkFacet *f1 = addFacet(0,3,1);
+		PfxGjkFacet *f2 = addFacet(0,2,3);
+		PfxGjkFacet *f3 = addFacet(1,3,2);
+
+		if(SCE_PFX_UNLIKELY(!f0 || !f1 || !f2 || !f3)) return distance;
+
+		linkFacets(f0,0,f1,2);
+		linkFacets(f0,1,f3,2);
+		linkFacets(f0,2,f2,0);
+		linkFacets(f1,0,f2,2);
+		linkFacets(f1,1,f3,0);
+		linkFacets(f2,1,f3,1);
+	}
+
+	// 探索
+	PfxGjkFacet *facetMin = NULL;
+
+	do {
+		// 原点から一番近い点を算出し、そのベクトルと支点を返す
+		int minFacetIdx = 0;
+		{
+			PfxFloat minDistSqr = SCE_PFX_FLT_MAX;
+			for(int i=0;i<numFacetsHead;i++) {
+				if(facetsHead[i]->obsolete==0 && facetsHead[i]->distSqr < minDistSqr) {
+					minDistSqr = facetsHead[i]->distSqr;
+					facetMin = facetsHead[i];
+					minFacetIdx = i;
+				}
+			}
+		}
+
+		// リストからはずす
+		facetsHead[minFacetIdx] = facetsHead[--numFacetsHead];
+
+		PfxVector3 pInA(0.0f),qInB(0.0f);
+		getSupportVertexShapeA(shapeA,invRotA * facetMin->normal,pInA);
+		getSupportVertexShapeB(shapeB,invRotB * (-facetMin->normal),qInB);
+		PfxVector3 p = transformA.getTranslation() + transformA.getUpper3x3() * pInA;
+		PfxVector3 q = transformB.getTranslation() + transformB.getUpper3x3() * qInB;
+		PfxVector3 w = p - q;
+		PfxVector3 v = facetMin->closest;
+
+		// 最近接点チェック
+		PfxFloat l0 = length(v);
+		PfxFloat l1 = dot(facetMin->normal,w);
+
+		if((l1 - l0) < SCE_PFX_GJK_EPSILON) {
+			break;
+		}
+
+		// 求めた点を追加して面を分割
+		{
+			SCE_PFX_ASSERT(numVerts<(int)MAX_VERTS);
+			int vId = numVerts++;
+			vertsP[vId] = p;
+			vertsQ[vId] = q;
+			vertsW[vId] = w;
+
+			facetMin->obsolete = 1;
+
+			numEdges = 0;
+
+			silhouette(facetMin->adj[0],facetMin->j[0],w);
+			silhouette(facetMin->adj[1],facetMin->j[1],w);
+			silhouette(facetMin->adj[2],facetMin->j[2],w);
+
+			if(SCE_PFX_UNLIKELY(numEdges == 0)) break;
+
+			bool edgeCheck = true;
+			PfxGjkFacet *firstFacet,*lastFacet;
+			{
+				PfxGjkEdge &edge = edges[0];
+				int v0 = edge.f->v[(edge.i+1)%3];
+				int v1 = edge.f->v[edge.i];
+				firstFacet = addFacet(v0,v1,vId);
+				if(SCE_PFX_UNLIKELY(!firstFacet)) {
+					edgeCheck = false;
+					break;
+				}
+				linkFacets(edge.f,edge.i,firstFacet,0);
+				lastFacet = firstFacet;
+			}
+
+			if(SCE_PFX_UNLIKELY(!edgeCheck)) break;
+
+			for(int e=1;e<numEdges;e++) {
+				PfxGjkEdge &edge = edges[e];
+				int v0 = edge.f->v[(edge.i+1)%3];
+				int v1 = edge.f->v[edge.i];
+				PfxGjkFacet *f = addFacet(v0,v1,vId);
+				if(SCE_PFX_UNLIKELY(!f)) {edgeCheck=false;break;}
+				linkFacets(edge.f,edge.i,f,0);
+				linkFacets(f,2,lastFacet,1);
+				lastFacet = f;
+			}
+			if(SCE_PFX_UNLIKELY(!edgeCheck)) break;
+
+			linkFacets(lastFacet,1,firstFacet,2);
+		}
+
+		epaIterationCount++;
+		if(SCE_PFX_UNLIKELY(epaIterationCount > SCE_PFX_EPA_ITERATION_MAX || numFacetsHead == 0)) {
+			break;
+		}
+	} while(1);
+
+	// 衝突点計算
+	int v1 = facetMin->v[0];
+	int v2 = facetMin->v[1];
+	int v3 = facetMin->v[2];
+
+	PfxVector3 p0 = vertsW[v2]-vertsW[v1];
+	PfxVector3 p1 = vertsW[v3]-vertsW[v1];
+	PfxVector3 p2 = facetMin->closest-vertsW[v1];
+
+	PfxVector3 v = cross( p0, p1 );
+	PfxVector3 crS = cross( v, p0 );
+	PfxVector3 crT = cross( v, p1 );
+	PfxFloat d0 = dot( crT, p0 );
+	PfxFloat d1 = dot( crS, p1 );
+
+	if(fabsf(d0) < SCE_PFX_GJK_EPSILON || fabsf(d1) < SCE_PFX_GJK_EPSILON) return distance;
+
+	PfxFloat lamda1 = dot( crT, p2 ) / d0;
+	PfxFloat lamda2 = dot( crS, p2 ) / d1;
+
+	pA = vertsP[v1] + lamda1*(vertsP[v2]-vertsP[v1]) + lamda2*(vertsP[v3]-vertsP[v1]);
+	pB = vertsQ[v1] + lamda1*(vertsQ[v2]-vertsQ[v1]) + lamda2*(vertsQ[v3]-vertsQ[v1]);
+
+	PfxFloat lenSqr = lengthSqr(pB-pA);
+	normal = normalize(pB-pA);
+
+	return -sqrtf(lenSqr);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Gjk
+
+PfxFloat PfxGjkSolver::collide( PfxVector3& normal, PfxPoint3 &pointA, PfxPoint3 &pointB,
+						const PfxTransform3 & transformA,
+						const PfxTransform3 & transformB,
+						PfxFloat distanceThreshold)
+{
+	(void) distanceThreshold;
+
+	int gjkIterationCount = 0;
+
+	m_simplex.reset();
+
+	PfxTransform3 cTransformA = transformA;
+	PfxTransform3 cTransformB = transformB;
+	PfxMatrix3 invRotA = transpose(cTransformA.getUpper3x3());
+	PfxMatrix3 invRotB = transpose(cTransformB.getUpper3x3());
+
+	PfxVector3 offset = (cTransformA.getTranslation() + cTransformB.getTranslation())*0.5f;
+	cTransformA.setTranslation(cTransformA.getTranslation()-offset);
+	cTransformB.setTranslation(cTransformB.getTranslation()-offset);
+
+	PfxVector3 separatingAxis(-cTransformA.getTranslation());
+	if(lengthSqr(separatingAxis) < 0.000001f) separatingAxis = PfxVector3(1.0,0.0,0.0);
+	PfxFloat squaredDistance = SCE_PFX_FLT_MAX;
+	PfxFloat delta = 0.0f;
+	PfxFloat distance = SCE_PFX_FLT_MAX;
+
+	for(;;) {
+		// サポート頂点の取得
+		PfxVector3 pInA,qInB;
+
+		getSupportVertexShapeA(shapeA,invRotA * (-separatingAxis),pInA);
+		getSupportVertexShapeB(shapeB,invRotB * separatingAxis,qInB);
+
+		PfxVector3 p = cTransformA.getTranslation() + cTransformA.getUpper3x3() * pInA;
+		PfxVector3 q = cTransformB.getTranslation() + cTransformB.getUpper3x3() * qInB;
+		PfxVector3 w = p - q;
+
+		delta = dot(separatingAxis,w);
+
+		// 早期終了チェック
+		if(SCE_PFX_UNLIKELY(delta > 0.0f)) {
+			normal = separatingAxis;
+			return distance;
+		}
+
+		if(SCE_PFX_UNLIKELY(m_simplex.inSimplex(w))) {
+			break;
+		}
+
+		PfxFloat f0 = squaredDistance - delta;
+		PfxFloat f1 = squaredDistance * SCE_PFX_GJK_EPSILON;
+
+		if (SCE_PFX_UNLIKELY(f0 <= f1)) {
+			break;
+		}
+
+		// 頂点を単体に追加
+		m_simplex.addVertex(w,p,q);
+		
+		// 原点と単体の最近接点を求め、分離軸を返す
+		if(SCE_PFX_UNLIKELY(!m_simplex.closest(separatingAxis))) {
+			normal = separatingAxis;
+			return distance;
+		}
+
+		squaredDistance = lengthSqr(separatingAxis);
+
+		if(SCE_PFX_UNLIKELY(gjkIterationCount >= SCE_PFX_GJK_ITERATION_MAX || m_simplex.fullSimplex())) {
+			break;
+		}
+
+		gjkIterationCount++;
+	}
+
+	PfxVector3 pA(0.0f),pB(0.0f);
+
+	// ２つのConvexは交差しているので、接触点を探索する
+	PfxFloat dist = detectPenetrationDepth(cTransformA,cTransformB,pA,pB,normal);
+
+	//マージン考慮
+	if(dist < 0.0f) {
+		pA += normal * SCE_PFX_GJK_MARGIN;
+		pB -= normal * SCE_PFX_GJK_MARGIN;
+		dist = dot(normal,pA-pB);
+		pointA = orthoInverse(transformA)*PfxPoint3(pA+offset);
+		pointB = orthoInverse(transformB)*PfxPoint3(pB+offset);
+	}
+
+	return dist;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_gjk_solver.h

@@ -0,0 +1,190 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_GJK_SOLVER_H
+#define _SCE_PFX_GJK_SOLVER_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+#include "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"
+#include "pfx_simplex_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_GJK_EPSILON			1e-04f
+#define SCE_PFX_GJK_MARGIN			0.025f
+#define SCE_PFX_GJK_ITERATION_MAX	10
+#define SCE_PFX_EPA_ITERATION_MAX	10
+
+///////////////////////////////////////////////////////////////////////////////
+// Support Function
+
+typedef void (*PfxGetSupportVertexFunc)(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+
+///////////////////////////////////////////////////////////////////////////////
+// Gjk
+class PfxSimplexSolver;
+
+class PfxGjkSolver
+{
+private:
+	static const PfxUInt32 MAX_VERTS = 128;
+	static const PfxUInt32 MAX_EDGES = 128;
+	static const PfxUInt32 MAX_FACETS = 64;
+	
+	PfxSimplexSolver m_simplex;
+
+	// 面
+	struct PfxGjkFacet {
+		PfxVector3 normal;		// 面の法線
+		PfxVector3 closest;		// 原点からの最短ベクトル
+		PfxUInt32 obsolete;		// 廃棄面判定
+		PfxFloat distSqr;		// 原点からの距離
+		PfxInt32 v[3];			// 頂点
+		PfxInt32 j[3];			// 隣接面から見たIndex
+		PfxGjkFacet *adj[3];	// 隣接面
+		SCE_PFX_PADDING(1,4)
+	};
+	
+	// エッジ
+	struct PfxGjkEdge {
+		PfxGjkFacet *f;
+		PfxInt32 i;
+		PfxGjkEdge() {}
+		PfxGjkEdge(PfxGjkFacet *f_,PfxInt32 i_)
+		{
+			f = f_;
+			i= i_;
+		}
+	};
+	
+	PfxVector3 g_vertsP[MAX_VERTS];
+	PfxVector3 g_vertsQ[MAX_VERTS];
+	PfxVector3 g_vertsW[MAX_VERTS];
+	PfxGjkFacet g_facets[MAX_FACETS];
+	PfxGjkFacet *g_facetsHead[MAX_FACETS];
+	PfxGjkEdge  g_edges[MAX_EDGES];
+
+	PfxVector3 SCE_PFX_ALIGNED(16) *vertsP;
+	SCE_PFX_PADDING(1,12)
+	PfxVector3 SCE_PFX_ALIGNED(16) *vertsQ;
+	SCE_PFX_PADDING(2,12)
+	PfxVector3 SCE_PFX_ALIGNED(16) *vertsW;
+	SCE_PFX_PADDING(3,12)
+	PfxGjkFacet SCE_PFX_ALIGNED(16) *facets;
+	SCE_PFX_PADDING(4,12)
+	PfxGjkFacet SCE_PFX_ALIGNED(16) **facetsHead;
+	SCE_PFX_PADDING(5,12)
+	PfxGjkEdge  SCE_PFX_ALIGNED(16) *edges;
+	
+	int numVerts;
+	int numEdges;
+	int numFacets;
+	int numFacetsHead;
+	SCE_PFX_PADDING(6,12)
+	
+	inline PfxGjkFacet *addFacet(int v1,int v2,int v3);
+
+	inline void linkFacets(PfxGjkFacet *f1,int e1,PfxGjkFacet *f2,int e2);
+	void silhouette(PfxGjkFacet *facet,int i,PfxVector3 &w);
+
+	inline bool originInTetrahedron(const PfxVector3& p0,const PfxVector3& p1,const PfxVector3& p2,const PfxVector3& p3);
+
+	PfxFloat detectPenetrationDepth(
+		const PfxTransform3 &transformA,const PfxTransform3 &transformB,
+		PfxVector3 &pA,PfxVector3 &pB,PfxVector3 &normal);
+
+	void *shapeA;
+	void *shapeB;
+	PfxGetSupportVertexFunc getSupportVertexShapeA;
+	PfxGetSupportVertexFunc getSupportVertexShapeB;
+
+public:
+	PfxGjkSolver();
+	~PfxGjkSolver();
+	
+	void setup(void *sA,void *sB,PfxGetSupportVertexFunc fA,PfxGetSupportVertexFunc fB);
+	
+	PfxFloat collide( PfxVector3& normal, PfxPoint3 &pointA, PfxPoint3 &pointB,
+					const PfxTransform3 & transformA,
+					const PfxTransform3 & transformB,
+					PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);
+};
+
+inline
+PfxGjkSolver::PfxGjkFacet *PfxGjkSolver::addFacet(int v1,int v2,int v3)
+{
+	if(SCE_PFX_UNLIKELY(numFacets == (int)MAX_FACETS))
+		return NULL;
+
+	PfxGjkFacet &facet = facets[numFacets];
+
+	PfxVector3 V1 = vertsW[v1];
+	PfxVector3 V2 = vertsW[v2];
+	PfxVector3 V3 = vertsW[v3];
+	facet.obsolete = 0;
+	facet.v[0] = v1;
+	facet.v[1] = v2;
+	facet.v[2] = v3;
+
+	PfxVector3 normal = cross(V3-V1,V2-V1);
+	
+	PfxFloat l = lengthSqr(normal);
+
+	if(l < SCE_PFX_GJK_EPSILON * SCE_PFX_GJK_EPSILON) {
+		return NULL;
+	}
+
+	normal /= sqrtf(l);
+	facet.closest = dot(V1,normal)*normal;
+	facet.normal =normal;
+
+	facet.distSqr = lengthSqr(facet.closest);
+
+	facetsHead[numFacetsHead++] = &facet;
+	numFacets++;
+
+	return &facet;
+}
+
+inline
+void PfxGjkSolver::linkFacets(PfxGjkFacet *f1,int e1,PfxGjkFacet *f2,int e2)
+{
+	f1->adj[e1] = f2;
+	f2->adj[e2] = f1;
+	f1->j[e1] = e2;
+	f2->j[e2] = e1;
+}
+
+inline
+bool PfxGjkSolver::originInTetrahedron(const PfxVector3& p0,const PfxVector3& p1,const PfxVector3& p2,const PfxVector3& p3)
+{
+    PfxVector3 n0 = cross((p1-p0),(p2-p0));
+    PfxVector3 n1 = cross((p2-p1),(p3-p1));
+    PfxVector3 n2 = cross((p3-p2),(p0-p2));
+    PfxVector3 n3 = cross((p0-p3),(p1-p3));
+
+	return 
+		dot(n0,p0) * dot(n0,p3-p0) <= 0.0f &&
+		dot(n1,p1) * dot(n1,p0-p1) <= 0.0f &&
+		dot(n2,p2) * dot(n2,p1-p2) <= 0.0f &&
+		dot(n3,p3) * dot(n3,p2-p3) <= 0.0f;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_GJK_SOLVER_H

Extras/PhysicsEffects/src/base_level/collision/pfx_gjk_support_func.cpp

@@ -0,0 +1,134 @@
+/*
+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 "pfx_gjk_support_func.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_box.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_capsule.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_cylinder.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_sphere.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_GJK_MARGIN 0.025f
+
+///////////////////////////////////////////////////////////////////////////////
+// Support Function
+
+void pfxGetSupportVertexTriangle(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
+{
+	PfxVector3 *vtx = (PfxVector3*)shape;
+	
+PfxFloat d0 = dot(vtx[0],seperatingAxis);
+PfxFloat d1 = dot(vtx[1],seperatingAxis);
+PfxFloat d2 = dot(vtx[2],seperatingAxis);
+
+int reti = 2;
+
+if(d0 > d1 && d0 > d2) {
+	reti = 0;
+}
+else if(d1 > d2) {
+	reti = 1;
+}
+
+	supportVertex = vtx[reti] + SCE_PFX_GJK_MARGIN * normalize(seperatingAxis);
+}
+
+void pfxGetSupportVertexTriangleWithThickness(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
+{
+	PfxVector3 *vtx = (PfxVector3*)shape;
+	
+PfxFloat d[6];
+d[0] = dot(vtx[0],seperatingAxis);
+d[1] = dot(vtx[1],seperatingAxis);
+d[2] = dot(vtx[2],seperatingAxis);
+d[3] = dot(vtx[3],seperatingAxis);
+d[4] = dot(vtx[4],seperatingAxis);
+d[5] = dot(vtx[5],seperatingAxis);
+
+int reti = 0;
+for(int i=1;i<6;i++) {
+	if(d[reti] < d[i]) {
+		reti = i;
+	}
+}
+
+	supportVertex = vtx[reti] + SCE_PFX_GJK_MARGIN * normalize(seperatingAxis);
+}
+
+void pfxGetSupportVertexConvex(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
+{
+	PfxConvexMesh *mesh = (PfxConvexMesh*)shape;
+	int reti = 0;
+PfxFloat dmax = dot(mesh->m_verts[0],seperatingAxis);
+for(int i=1;i<mesh->m_numVerts;i++) {
+	PfxFloat d = dot(mesh->m_verts[i],seperatingAxis);
+	if(d > dmax) {
+		dmax =d;
+		reti = i;
+	}
+}
+	supportVertex = mesh->m_verts[reti] + SCE_PFX_GJK_MARGIN * normalize(seperatingAxis);
+}
+
+void pfxGetSupportVertexBox(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
+{
+	PfxBox *box = (PfxBox*)shape;
+	PfxVector3 boxHalf = box->m_half + PfxVector3(SCE_PFX_GJK_MARGIN);
+	supportVertex[0] = seperatingAxis[0]>0.0f?boxHalf[0]:-boxHalf[0];
+	supportVertex[1] = seperatingAxis[1]>0.0f?boxHalf[1]:-boxHalf[1];
+	supportVertex[2] = seperatingAxis[2]>0.0f?boxHalf[2]:-boxHalf[2];
+}
+
+void pfxGetSupportVertexCapsule(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
+{
+	PfxCapsule *capsule = (PfxCapsule*)shape;
+	PfxVector3 u(1.0f,0.0f,0.0f);
+
+	PfxFloat udotv = dot(seperatingAxis,u);
+	PfxVector3 dir = u * (udotv > 0.0f ? capsule->m_halfLen : -capsule->m_halfLen);
+	supportVertex = dir + normalize(seperatingAxis) * (capsule->m_radius + SCE_PFX_GJK_MARGIN);
+}
+
+void pfxGetSupportVertexSphere(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
+{
+	PfxSphere *sphere = (PfxSphere*)shape;
+	supportVertex = normalize(seperatingAxis) * (sphere->m_radius + SCE_PFX_GJK_MARGIN);
+}
+
+void pfxGetSupportVertexCylinder(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
+{
+	PfxCylinder *cylinder = (PfxCylinder*)shape;
+	PfxVector3 u(1.0f,0.0f,0.0f);
+
+	PfxFloat udotv = dot(seperatingAxis,u);
+	PfxFloat s = seperatingAxis[1]*seperatingAxis[1]+seperatingAxis[2]*seperatingAxis[2];
+	if(s < 0.000001f) {
+		supportVertex = u * (udotv > 0.0f ? cylinder->m_halfLen + SCE_PFX_GJK_MARGIN : -cylinder->m_halfLen-SCE_PFX_GJK_MARGIN);
+	}
+	else {
+		PfxVector3 dir = u * (udotv > 0.0f ? cylinder->m_halfLen : -cylinder->m_halfLen);
+		PfxVector3 vYZ = seperatingAxis;
+		vYZ[0] = 0.0f;
+		vYZ /= sqrtf(s);
+		supportVertex = dir + vYZ * (cylinder->m_radius) + normalize(seperatingAxis) * SCE_PFX_GJK_MARGIN;
+	}
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_gjk_support_func.h

@@ -0,0 +1,36 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_GJK_SUPPORT_FUNC_H
+#define _SCE_PFX_GJK_SUPPORT_FUNC_H
+
+namespace sce {
+namespace PhysicsEffects {
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+
+void pfxGetSupportVertexTriangle(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+void pfxGetSupportVertexTriangleWithThickness(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+void pfxGetSupportVertexConvex(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+void pfxGetSupportVertexBox(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+void pfxGetSupportVertexCapsule(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+void pfxGetSupportVertexSphere(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+void pfxGetSupportVertexCylinder(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_GJK_SUPPORT_FUNC_H

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_common.h

@@ -0,0 +1,427 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_COMMON_H
+#define _SCE_PFX_INTERSECT_COMMON_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_INTERSECT_COMMON_EPSILON 0.00001f
+#define SCE_PFX_RAY_TRIANGLE_EPSILON 0.00001f
+
+// Internally used intersect functions
+
+struct PfxTriangle
+{
+	PfxVector3 points[3];
+	
+	PfxTriangle(const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2)
+	{
+		points[0] = p0;
+		points[1] = p1;
+		points[2] = p2;
+	}
+};
+
+struct PfxPlane
+{
+	PfxVector3 normal; // normal
+	PfxVector3 point; // point on the plane
+	
+	PfxPlane(const PfxVector3 &n,const PfxVector3 &q)
+	{
+		normal = n;
+		point = q;
+	}
+	
+	PfxPlane(const PfxTriangle &triangle)
+	{
+		normal = normalize(cross(triangle.points[1]-triangle.points[0],triangle.points[2]-triangle.points[0]));
+		point = triangle.points[0];
+	}
+	
+	PfxFloat onPlane(const PfxVector3 &p) const
+	{
+		return dot((p-point),normal);
+	}
+};
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxIntersectRayAABBFast(
+	const PfxVector3 &rayStartPosition,
+	const PfxVector3 &rayDirection,
+	const PfxVector3 &AABBcenter,
+	const PfxVector3 &AABBhalf,
+	PfxFloat &variable)
+{
+	PfxVector3 AABBmin = AABBcenter - AABBhalf;
+	PfxVector3 AABBmax = AABBcenter + AABBhalf;
+	
+	PfxVector3 dir = rayDirection;
+	PfxVector3 absDir = absPerElem(dir);
+	PfxVector3 sign = copySignPerElem(PfxVector3(1.0),dir);
+	
+if(absDir[0] < SCE_PFX_INTERSECT_COMMON_EPSILON) {
+	if(rayStartPosition[0] < AABBmin[0] || rayStartPosition[0] > AABBmax[0]) {
+		return false;
+	}
+	dir[0] = sign[0] * SCE_PFX_INTERSECT_COMMON_EPSILON;
+}
+
+if(absDir[1] < SCE_PFX_INTERSECT_COMMON_EPSILON) {
+	if(rayStartPosition[1] < AABBmin[1] || rayStartPosition[1] > AABBmax[1]) {
+		return false;
+	}
+	dir[1] = sign[1] * SCE_PFX_INTERSECT_COMMON_EPSILON;
+}
+
+if(absDir[2] < SCE_PFX_INTERSECT_COMMON_EPSILON) {
+	if(rayStartPosition[2] < AABBmin[2] || rayStartPosition[2] > AABBmax[2]) {
+		return false;
+	}
+	dir[2] = sign[2] * SCE_PFX_INTERSECT_COMMON_EPSILON;
+}
+
+	PfxVector3 t1 = divPerElem(AABBmin - rayStartPosition, dir);
+	PfxVector3 t2 = divPerElem(AABBmax - rayStartPosition, dir);
+	
+	PfxVector3 tmin = minPerElem(t1,t2);
+	PfxVector3 tmax = maxPerElem(t1,t2);
+	
+if(maxElem(tmin) > minElem(tmax)) return false;
+
+if(tmin[0] > tmin[1]) {
+	if(tmin[0] > tmin[2]) {
+		variable = tmin[0];
+	}
+	else {
+		variable = tmin[2];
+	}
+}
+else {
+	if(tmin[1] > tmin[2]) {
+		variable = tmin[1];
+	}
+	else {
+		variable = tmin[2];
+	}
+}
+
+	return true;
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxIntersectRayAABB(
+	const PfxVector3 &rayStartPosition,
+	const PfxVector3 &rayDirection,
+	const PfxVector3 &AABBcenter,
+	const PfxVector3 &AABBhalf,
+	PfxFloat &variable,
+	PfxVector3 &normal)
+{
+	PfxVector3 AABBmin = AABBcenter - AABBhalf;
+	PfxVector3 AABBmax = AABBcenter + AABBhalf;
+	
+	PfxVector3 dir = rayDirection;
+	PfxVector3 absDir = absPerElem(dir);
+	PfxVector3 sign = copySignPerElem(PfxVector3(1.0),dir);
+
+	// 始点がBoxの内側にあるか判定
+	if( AABBmin[0] < rayStartPosition[0] && rayStartPosition[0] < AABBmax[0] &&
+		AABBmin[1] < rayStartPosition[1] && rayStartPosition[1] < AABBmax[1] &&
+		AABBmin[2] < rayStartPosition[2] && rayStartPosition[2] < AABBmax[2]) {
+		return false;
+	}
+
+if(absDir[0] < SCE_PFX_INTERSECT_COMMON_EPSILON) {
+	if(rayStartPosition[0] < AABBmin[0] || rayStartPosition[0] > AABBmax[0]) {
+		return false;
+	}
+	dir[0] = sign[0] * SCE_PFX_INTERSECT_COMMON_EPSILON;
+}
+
+if(absDir[1] < SCE_PFX_INTERSECT_COMMON_EPSILON) {
+	if(rayStartPosition[1] < AABBmin[1] || rayStartPosition[1] > AABBmax[1]) {
+		return false;
+	}
+	dir[1] = sign[1] * SCE_PFX_INTERSECT_COMMON_EPSILON;
+}
+
+if(absDir[2] < SCE_PFX_INTERSECT_COMMON_EPSILON) {
+	if(rayStartPosition[2] < AABBmin[2] || rayStartPosition[2] > AABBmax[2]) {
+		return false;
+	}
+	dir[2] = sign[2] * SCE_PFX_INTERSECT_COMMON_EPSILON;
+}
+
+	PfxVector3 t1 = divPerElem(AABBmin - rayStartPosition, dir);
+	PfxVector3 t2 = divPerElem(AABBmax - rayStartPosition, dir);
+	
+	PfxVector3 tmin = minPerElem(t1,t2);
+	PfxVector3 tmax = maxPerElem(t1,t2);
+	
+	normal = PfxVector3(0);
+
+if(maxElem(tmin) > minElem(tmax)) return false;
+
+if(tmin[0] > tmin[1]) {
+	if(tmin[0] > tmin[2]) {
+		variable = tmin[0];
+		normal[0] = -sign[0];
+	}
+	else {
+		variable = tmin[2];
+		normal[2] = -sign[2];
+	}
+}
+else {
+	if(tmin[1] > tmin[2]) {
+		variable = tmin[1];
+		normal[1] = -sign[1];
+	}
+	else {
+		variable = tmin[2];
+		normal[2] = -sign[2];
+	}
+}
+
+	return true;
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxClosestTwoLines(
+	const PfxVector3 &p1,const PfxVector3 &q1, // line1
+	const PfxVector3 &p2,const PfxVector3 &q2, // line2
+	PfxVector3 &s1,PfxVector3 &s2)
+{
+	PfxVector3 v1 = q1-p1;
+	PfxVector3 v2 = q2-p2;
+	PfxVector3 r = p1 - p2;
+
+	PfxFloat a = dot(v1,v1);
+	PfxFloat e = dot(v2,v2);
+	PfxFloat f = dot(v2,r);
+	PfxFloat b = dot(v1,v2);
+	PfxFloat c = dot(v1,r);
+	PfxFloat den = a*e-b*b;
+	
+	PfxFloat s,t;
+	
+	if(den != 0.0f) {
+		s = SCE_PFX_CLAMP((b*f-c*e)/den,0.0f,1.0f);
+	}
+	else {
+		s = 0.0f;
+	}
+	
+	t = (b*s+f)/e;
+	
+	if(t < 0.0f) {
+		t = 0.0f;
+		s = SCE_PFX_CLAMP(-c/a,0.0f,1.0f);
+	}
+	else if(t > 1.0f) {
+		t = 1.0f;
+		s = SCE_PFX_CLAMP((b-c)/a,0.0f,1.0f);
+	}
+	
+	s1 = p1 + s * v1;
+	s2 = p2 + t * v2;
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxClosestPointAABB(
+	const PfxVector3 &point,
+	const PfxVector3 &AABBhalf,
+	PfxVector3 &s)
+{
+	s = point;
+	s = maxPerElem(s,-AABBhalf);
+	s = minPerElem(s,AABBhalf);
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxClosestPointTriangle(
+	const PfxVector3 &point,
+	const PfxTriangle &triangle,
+	PfxVector3 &s)
+{
+	PfxVector3 a = triangle.points[0];
+	PfxVector3 b = triangle.points[1];
+	PfxVector3 c = triangle.points[2];
+    PfxVector3 ab = b - a;
+    PfxVector3 ac = c - a;
+    PfxVector3 ap = point - a;
+    PfxFloat d1 = dot(ab, ap);
+    PfxFloat d2 = dot(ac, ap);
+	if(d1 <= 0.0f && d2 <= 0.0f) {
+		s = a;
+		return;
+	}
+
+    PfxVector3 bp = point - b;
+    PfxFloat d3 = dot(ab, bp);
+    PfxFloat d4 = dot(ac, bp);
+	if (d3 >= 0.0f && d4 <= d3) {
+		s = b;
+		return;
+	}
+
+    PfxFloat vc = d1*d4 - d3*d2;
+    if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
+        PfxFloat v = d1 / (d1 - d3);
+        s = a + v * ab;
+		return;
+    }
+
+    PfxVector3 cp = point - c;
+    PfxFloat d5 = dot(ab, cp);
+    PfxFloat d6 = dot(ac, cp);
+	if (d6 >= 0.0f && d5 <= d6) {
+		s = c;
+		return;
+	}
+
+    PfxFloat vb = d5*d2 - d1*d6;
+    if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
+        PfxFloat w = d2 / (d2 - d6);
+        s = a + w * ac;
+		return;
+    }
+
+    PfxFloat va = d3*d6 - d5*d4;
+    if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
+        PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+        s = b + w * (c - b);
+		return;
+    }
+
+    PfxFloat den = 1.0f / (va + vb + vc);
+    PfxFloat v = vb * den;
+    PfxFloat w = vc * den;
+    s = a + ab * v + ac * w;
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxIntersectRayTriangle(
+	const PfxVector3 &rayStartPosition,
+	const PfxVector3 &rayDirection,
+	const PfxTriangle &triangle,
+	PfxFloat &variable)
+{
+	PfxFloat v,w;
+	PfxVector3 ab = triangle.points[1] - triangle.points[0];
+	PfxVector3 ac = triangle.points[2] - triangle.points[0];
+
+	PfxVector3 n = cross(ab,ac);
+
+	PfxFloat d = dot(-rayDirection,n);
+	
+	if(fabsf(d) < 0.00001f) return false;
+
+	PfxVector3 ap = rayStartPosition - triangle.points[0];
+	PfxFloat t = dot(ap,n) / d;
+
+	if(t <= 0.0f || t >= 1.0f) return false;
+
+	variable = t;
+
+	PfxVector3 e = cross(-rayDirection,ap);
+	v = dot(ac,e) / d;
+	if(v < -SCE_PFX_RAY_TRIANGLE_EPSILON || v > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;
+
+	w = -dot(ab,e) / d;
+	if(w < -SCE_PFX_RAY_TRIANGLE_EPSILON || v+w > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;
+
+	return true;
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxIntersectRayTriangleWithoutFrontFace(
+	const PfxVector3 &rayStartPosition,
+	const PfxVector3 &rayDirection,
+	const PfxTriangle &triangle,
+	PfxFloat &variable)
+{
+	PfxFloat v,w;
+	PfxVector3 ab = triangle.points[1] - triangle.points[0];
+	PfxVector3 ac = triangle.points[2] - triangle.points[0];
+
+	PfxVector3 n = cross(ab,ac);
+
+	PfxFloat d = dot(-rayDirection,n);
+	
+	if(d >= 0.0f) return false;
+
+	PfxVector3 ap = rayStartPosition - triangle.points[0];
+	PfxFloat t = dot(ap,n) / d;
+
+	if(t <= 0.0f || t >= 1.0f) return false;
+
+	variable = t;
+
+	PfxVector3 e = cross(-rayDirection,ap);
+	v = dot(ac,e) / d;
+	if(v < -SCE_PFX_RAY_TRIANGLE_EPSILON || v > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;
+
+	w = -dot(ab,e) / d;
+	if(w < -SCE_PFX_RAY_TRIANGLE_EPSILON || v+w > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;
+
+	return true;
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxIntersectRayTriangleWithoutBackFace(
+	const PfxVector3 &rayStartPosition,
+	const PfxVector3 &rayDirection,
+	const PfxTriangle &triangle,
+	PfxFloat &variable)
+{
+	PfxFloat v,w;
+	PfxVector3 ab = triangle.points[1] - triangle.points[0];
+	PfxVector3 ac = triangle.points[2] - triangle.points[0];
+
+	PfxVector3 n = cross(ab,ac);
+
+	PfxFloat d = dot(-rayDirection,n);
+	
+	if(d <= 0.0f) return false;
+
+	PfxVector3 ap = rayStartPosition - triangle.points[0];
+	PfxFloat t = dot(ap,n) / d;
+
+	if(t <= 0.0f || t >= 1.0f) return false;
+
+	variable = t;
+
+	PfxVector3 e = cross(-rayDirection,ap);
+	v = dot(ac,e) / d;
+	if(v < -SCE_PFX_RAY_TRIANGLE_EPSILON || v > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;
+
+	w = -dot(ab,e) / d;
+	if(w < -SCE_PFX_RAY_TRIANGLE_EPSILON || v+w > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;
+
+	return true;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+
+#endif // _SCE_PFX_INTERSECT_COMMON_H

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_box.cpp

@@ -0,0 +1,47 @@
+/*
+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 "pfx_intersect_common.h"
+#include "pfx_intersect_ray_box.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayBox(const PfxRayInput &ray,PfxRayOutput &out,const PfxBox &box,const PfxTransform3 &transform)
+{
+	// レイをBoxのローカル座標へ変換
+	PfxTransform3 transformBox = orthoInverse(transform);
+	PfxVector3 rayStartPosition = transformBox.getUpper3x3() * ray.m_startPosition + transformBox.getTranslation();
+	PfxVector3 rayDirection = transformBox.getUpper3x3() * ray.m_direction;
+	
+	// 交差判定
+	PfxFloat tmpVariable=0.0f;
+	PfxVector3 tmpNormal(0.0f);
+	if(pfxIntersectRayAABB(rayStartPosition,rayDirection,PfxVector3(0.0f),box.m_half,tmpVariable,tmpNormal)) {
+		if(tmpVariable > 0.0f && tmpVariable < out.m_variable) {
+			out.m_contactFlag = true;
+			out.m_variable = tmpVariable;
+			out.m_contactPoint = ray.m_startPosition + tmpVariable * ray.m_direction;
+			out.m_contactNormal = transform.getUpper3x3() * tmpNormal;
+			out.m_subData.m_type = PfxSubData::NONE;
+			return true;
+		}
+	}
+	
+	return false;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_box.h

@@ -0,0 +1,31 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_RAYBOX_H
+#define _SCE_PFX_INTERSECT_RAYBOX_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_box.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayBox(const PfxRayInput &ray,PfxRayOutput &out,const PfxBox &box,const PfxTransform3 &transform);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_INTERSECT_RAYBOX_H

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_capsule.cpp

@@ -0,0 +1,136 @@
+/*
+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 "pfx_intersect_common.h"
+#include "pfx_intersect_ray_capsule.h"
+
+namespace sce {
+namespace PhysicsEffects {
+PfxBool pfxIntersectRayCapsule(const PfxRayInput &ray,PfxRayOutput &out,const PfxCapsule &capsule,const PfxTransform3 &transform)
+{
+	// レイをCapsuleのローカル座標へ変換
+	PfxTransform3 transformCapsule = orthoInverse(transform);
+	PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();
+	PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;
+	
+	PfxFloat radSqr = capsule.m_radius * capsule.m_radius;
+
+	// 始点がカプセルの内側にあるか判定
+	{
+		PfxFloat h = fabsf(startPosL[0]);
+		if(h > capsule.m_halfLen) h = capsule.m_halfLen;
+		PfxVector3 Px(out.m_variable,0,0);
+		PfxFloat sqrLen = lengthSqr(startPosL-Px);
+		if(sqrLen <= radSqr) return false;
+	}
+
+	// カプセルの胴体との交差判定
+	do {
+		PfxVector3 P(startPosL);
+		PfxVector3 D(rayDirL);
+		
+		P[0] = 0.0f;
+		D[0] = 0.0f;
+		
+		PfxFloat a = dot(D,D);
+		PfxFloat b = dot(P,D);
+		PfxFloat c = dot(P,P) - radSqr;
+		
+		PfxFloat d = b * b - a * c;
+		
+		if(d < 0.0f || fabs(a) < 0.00001f) return false;
+		
+		PfxFloat tt = ( -b - sqrtf(d) ) / a;
+		
+		if(tt < 0.0f)
+			break;
+		else if(tt > 1.0f)
+			return false;
+		
+		if(tt < out.m_variable) {
+			PfxVector3 cp = startPosL + tt * rayDirL;
+			
+			if(fabsf(cp[0]) <= capsule.m_halfLen) {
+				out.m_contactFlag = true;
+				out.m_variable = tt;
+				out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));
+				out.m_contactNormal = transform.getUpper3x3() * normalize(cp);
+				out.m_subData.m_type = PfxSubData::NONE;
+				return true;
+			}
+		}
+	} while(0);
+	
+	// カプセルの両端にある球体との交差判定
+	PfxFloat a = dot(rayDirL,rayDirL);
+	if(fabs(a) < 0.00001f) return false;
+	
+	do {
+		PfxVector3 center(capsule.m_halfLen,0.0f,0.0f);
+		PfxVector3 v = startPosL - center;
+
+		PfxFloat b = dot(v,rayDirL);
+		PfxFloat c = dot(v,v) - radSqr;
+
+		PfxFloat d = b * b - a * c;
+		
+		if(d < 0.0f) break;
+		
+		PfxFloat tt = ( -b - sqrtf(d) ) / a;
+		
+		if(tt < 0.0f || tt > 1.0f) break;
+		
+		if(tt < out.m_variable) {
+			PfxVector3 cp = startPosL + tt * rayDirL;
+			out.m_contactFlag = true;
+			out.m_variable = tt;
+			out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
+			out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);
+			out.m_subData.m_type = PfxSubData::NONE;
+			return true;
+		}
+	} while(0);
+	
+	{
+		PfxVector3 center(-capsule.m_halfLen,0.0f,0.0f);
+		PfxVector3 v = startPosL - center;
+
+		PfxFloat b = dot(v,rayDirL);
+		PfxFloat c = dot(v,v) - radSqr;
+
+		PfxFloat d = b * b - a * c;
+		
+		if(d < 0.0f) return false;
+		
+		PfxFloat tt = ( -b - sqrtf(d) ) / a;
+		
+		if(tt < 0.0f || tt > 1.0f) return false;
+		
+		if(tt < out.m_variable) {
+			PfxVector3 cp = startPosL + out.m_variable * rayDirL;
+			out.m_contactFlag = true;
+			out.m_variable = tt;
+			out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
+			out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);
+			out.m_subData.m_type = PfxSubData::NONE;
+			return true;
+		}
+	}
+	
+	return false;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_capsule.h

@@ -0,0 +1,31 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_RAYCAPSULE_H
+#define _SCE_PFX_INTERSECT_RAYCAPSULE_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_capsule.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayCapsule(const PfxRayInput &ray,PfxRayOutput &out,const PfxCapsule &capsule,const PfxTransform3 &transform);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_INTERSECT_RAYCAPSULE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_convex.cpp

@@ -0,0 +1,56 @@
+/*
+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/collision/pfx_tri_mesh.h"
+#include "pfx_intersect_common.h"
+#include "pfx_intersect_ray_convex.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayConvex(const PfxRayInput &ray,PfxRayOutput &out,const void *shape,const PfxTransform3 &transform)
+{
+	const PfxConvexMesh *convex = (const PfxConvexMesh*)shape;
+	
+	// レイをConvexのローカル座標へ変換
+	PfxTransform3 transformConvex = orthoInverse(transform);
+	PfxVector3 startPosL = transformConvex.getUpper3x3() * ray.m_startPosition + transformConvex.getTranslation();
+	PfxVector3 rayDirL = transformConvex.getUpper3x3() * ray.m_direction;
+
+	// レイとConvexの交差判定
+	PfxFloat tmpVariable(0.0f);
+	PfxVector3 tmpNormal;
+	bool ret = false;
+	for(PfxUInt32 f=0;f<(PfxUInt32)convex->m_numIndices/3;f++) {
+		PfxTriangle triangle(
+			convex->m_verts[convex->m_indices[f*3  ]],
+			convex->m_verts[convex->m_indices[f*3+1]],
+			convex->m_verts[convex->m_indices[f*3+2]]);
+
+		if(pfxIntersectRayTriangleWithoutBackFace(startPosL,rayDirL,triangle,tmpVariable) && tmpVariable < out.m_variable) {
+			out.m_contactFlag = true;
+			out.m_variable = tmpVariable;
+			out.m_contactPoint = ray.m_startPosition + tmpVariable * ray.m_direction;
+			out.m_contactNormal = transform.getUpper3x3() * normalize(cross(triangle.points[2]-triangle.points[1],triangle.points[0]-triangle.points[1]));
+			out.m_subData.m_type = PfxSubData::NONE;
+			ret = true;
+		}
+	}
+
+	return ret;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_convex.h

@@ -0,0 +1,30 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_RAYCONVEX_H
+#define _SCE_PFX_INTERSECT_RAYCONVEX_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayConvex(const PfxRayInput &ray,PfxRayOutput &out,const void *shape,const PfxTransform3 &transform);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_INTERSECT_RAYCONVEX_H

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_cylinder.cpp

@@ -0,0 +1,106 @@
+/*
+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 "pfx_intersect_common.h"
+#include "pfx_intersect_ray_cylinder.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayCylinder(const PfxRayInput &ray,PfxRayOutput &out,const PfxCylinder &cylinder,const PfxTransform3 &transform)
+{
+	// レイを円柱のローカル座標へ変換
+	PfxTransform3 transformCapsule = orthoInverse(transform);
+	PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();
+	PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;
+	
+	PfxFloat radSqr = cylinder.m_radius * cylinder.m_radius;
+
+	// 始点が円柱の内側にあるか判定
+	{
+		PfxFloat h = fabsf(startPosL[0]);
+		if(h > cylinder.m_halfLen) h = cylinder.m_halfLen;
+		PfxVector3 Px(out.m_variable,0,0);
+		PfxFloat sqrLen = lengthSqr(startPosL-Px);
+		if(sqrLen <= radSqr) return false;
+	}
+
+	// 円柱の胴体との交差判定
+	do {
+		PfxVector3 P(startPosL);
+		PfxVector3 D(rayDirL);
+		
+		P[0] = 0.0f;
+		D[0] = 0.0f;
+		
+		PfxFloat a = dot(D,D);
+		PfxFloat b = dot(P,D);
+		PfxFloat c = dot(P,P) - radSqr;
+		
+		PfxFloat d = b * b - a * c;
+		
+		if(d < 0.0f || fabs(a) < 0.00001f) return false;
+		
+		PfxFloat tt = ( -b - sqrtf(d) ) / a;
+		
+		if(tt < 0.0f)
+			break;
+		else if(tt > 1.0f)
+			return false;
+		
+		if(tt < out.m_variable) {
+			PfxVector3 cp = startPosL + tt * rayDirL;
+			
+			if(fabsf(cp[0]) <= cylinder.m_halfLen) {
+				out.m_contactFlag = true;
+				out.m_variable = tt;
+				out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));
+				out.m_contactNormal = transform.getUpper3x3() * normalize(cp);
+				out.m_subData.m_type = PfxSubData::NONE;
+				return true;
+			}
+		}
+	} while(0);
+	
+	// 円柱の両端にある平面との交差判定
+	{
+		if(fabsf(rayDirL[0]) < 0.00001f) return false;
+		
+		PfxFloat t1 = ( cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];
+		PfxFloat t2 = ( - cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];
+
+		PfxFloat tt = SCE_PFX_MIN(t1,t2);
+		
+		if(tt < 0.0f || tt > 1.0f) return false;
+
+		PfxVector3 p = startPosL + tt * rayDirL;
+		p[0] = 0.0f;
+
+		if(lengthSqr(p) < radSqr && tt < out.m_variable) {
+			PfxVector3 cp = startPosL + tt * rayDirL;
+			out.m_contactFlag = true;
+			out.m_variable = tt;
+			out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
+			out.m_contactNormal = transform.getUpper3x3() * ((cp[0]>0.0f)?PfxVector3(1.0,0.0,0.0):PfxVector3(-1.0,0.0,0.0));
+			out.m_subData.m_type = PfxSubData::NONE;
+			return true;
+		}
+	}
+	
+	return false;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_cylinder.h

@@ -0,0 +1,31 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_RAYCYLINDER_H
+#define _SCE_PFX_INTERSECT_RAYCYLINDER_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_cylinder.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayCylinder(const PfxRayInput &ray,PfxRayOutput &out,const PfxCylinder &cylinder,const PfxTransform3 &transform);
+
+} //namespace PhysicsEffects
+} //namespace sce
+ 
+#endif // _SCE_PFX_INTERSECT_RAYCYLINDER_H

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_large_tri_mesh.cpp

@@ -0,0 +1,169 @@
+/*
+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/collision/pfx_large_tri_mesh.h"
+#include "pfx_intersect_common.h"
+#include "pfx_mesh_common.h"
+#include "pfx_intersect_ray_large_tri_mesh.h"
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxIntersectRayTriMesh(const PfxTriMesh &mesh,const PfxVector3 &rayStart,const PfxVector3 &rayDir,
+	PfxUInt32 facetMode,PfxFloat &variable,PfxVector3 &normal,PfxSubData &subData)
+{
+	bool ret = false;
+	PfxFloat nearest_t = variable;
+	PfxVector3 nearest_nml(0.0f);
+	PfxUInt32 nearest_f = 0;
+
+	for(PfxUInt32 f=0;f<mesh.m_numFacets;f++) {
+		const PfxFacet &facet = mesh.m_facets[f];
+		
+		PfxVector3 facetCenter = pfxReadVector3(facet.m_center);
+		PfxVector3 facetHalf = pfxReadVector3(facet.m_half);
+
+		PfxFloat cur_t = 1.0f;
+
+		if( !pfxIntersectRayAABBFast(rayStart,rayDir,facetCenter,facetHalf,cur_t) )
+			continue;
+		
+		if( nearest_t <= cur_t ) continue;
+		
+		PfxTriangle triangle(
+			mesh.m_verts[facet.m_vertIds[0]],
+			mesh.m_verts[facet.m_vertIds[1]],
+			mesh.m_verts[facet.m_vertIds[2]]);
+		
+		if(facetMode == SCE_PFX_RAY_FACET_MODE_FRONT_ONLY && 
+			pfxIntersectRayTriangleWithoutBackFace(rayStart,rayDir,triangle,cur_t) && cur_t < nearest_t) {
+			nearest_f = f;
+			nearest_t = cur_t;
+			nearest_nml = pfxReadVector3(facet.m_normal);
+			ret = true;
+		}
+		else if(facetMode == SCE_PFX_RAY_FACET_MODE_BACK_ONLY && 
+			pfxIntersectRayTriangleWithoutFrontFace(rayStart,rayDir,triangle,cur_t) && cur_t < nearest_t) {
+			nearest_f = f;
+			nearest_t = cur_t;
+			nearest_nml = pfxReadVector3(facet.m_normal);
+			ret = true;
+		}
+		else if(facetMode == SCE_PFX_RAY_FACET_MODE_FRONT_AND_BACK && 
+			pfxIntersectRayTriangle(rayStart,rayDir,triangle,cur_t) && cur_t < nearest_t) {
+			nearest_f = f;
+			nearest_t = cur_t;
+			nearest_nml = pfxReadVector3(facet.m_normal);
+			ret = true;
+		}
+	}
+	
+	if(ret) {
+		// 面のローカル座標を算出
+		const PfxFacet &facet = mesh.m_facets[nearest_f];
+
+		PfxTriangle triangle(
+			mesh.m_verts[facet.m_vertIds[0]],
+			mesh.m_verts[facet.m_vertIds[1]],
+			mesh.m_verts[facet.m_vertIds[2]]);
+		
+		PfxFloat s=0.0f,t=0.0f;
+		pfxGetLocalCoords(rayStart+t*rayDir,triangle,s,t);
+		
+		subData.m_type = PfxSubData::MESH_INFO;
+		subData.setFacetLocalS(s);
+		subData.setFacetLocalT(t);
+		subData.setFacetId(nearest_f);
+		
+		variable = nearest_t;
+		normal = nearest_nml;
+	}
+
+	return ret;
+}
+
+PfxBool pfxIntersectRayLargeTriMesh(const PfxRayInput &ray,PfxRayOutput &out,const void *shape,const PfxTransform3 &transform)
+{
+	PfxBool ret = false;
+
+	const PfxLargeTriMesh &largeMesh = *((PfxLargeTriMesh*)shape);
+	
+	// レイをラージメッシュのローカル座標へ変換
+	PfxTransform3 transformLMesh = orthoInverse(transform);
+	PfxVector3 rayStartPosition = transformLMesh.getUpper3x3() * ray.m_startPosition + transformLMesh.getTranslation();
+	PfxVector3 rayDirection = transformLMesh.getUpper3x3() * ray.m_direction;
+	
+	PfxVecInt3 s,e,aabbMinL,aabbMaxL;
+
+	s = largeMesh.getLocalPosition(rayStartPosition);
+	e = largeMesh.getLocalPosition(rayStartPosition+rayDirection);
+
+	aabbMinL = minPerElem(s,e);
+	aabbMaxL = maxPerElem(s,e);
+	
+	PfxUInt32 numIslands = largeMesh.m_numIslands;
+	
+	{
+	for(PfxUInt32 i=0;i<numIslands;i++) {
+		PfxAabb16 aabbB = largeMesh.m_aabbList[i];
+		if(aabbMaxL.getX() < pfxGetXMin(aabbB) || aabbMinL.getX() > pfxGetXMax(aabbB)) continue;
+		if(aabbMaxL.getY() < pfxGetYMin(aabbB) || aabbMinL.getY() > pfxGetYMax(aabbB)) continue;
+		if(aabbMaxL.getZ() < pfxGetZMin(aabbB) || aabbMinL.getZ() > pfxGetZMax(aabbB)) continue;
+		// Todo:早期終了チェック
+
+			PfxVector3 aabbMin,aabbMax;
+			aabbMin = largeMesh.getWorldPosition(PfxVecInt3((PfxFloat)pfxGetXMin(aabbB),(PfxFloat)pfxGetYMin(aabbB),(PfxFloat)pfxGetZMin(aabbB)));
+			aabbMax = largeMesh.getWorldPosition(PfxVecInt3((PfxFloat)pfxGetXMax(aabbB),(PfxFloat)pfxGetYMax(aabbB),(PfxFloat)pfxGetZMax(aabbB)));
+
+			PfxFloat tmpVariable = 1.0f;
+			PfxVector3 tmpNormal;
+
+			if( !pfxIntersectRayAABBFast(
+				rayStartPosition,rayDirection,
+				(aabbMax+aabbMin)*0.5f,
+				(aabbMax-aabbMin)*0.5f,
+				tmpVariable) )
+				continue;
+			
+			if( out.m_variable <= tmpVariable ) continue;
+
+			// アイランドとの交差チェック
+		const PfxTriMesh *island = &largeMesh.m_islands[i];
+			
+			PfxSubData subData;
+			tmpVariable = out.m_variable;
+
+			if( pfxIntersectRayTriMesh(*island,rayStartPosition,rayDirection,ray.m_facetMode,tmpVariable,tmpNormal,subData) &&
+				tmpVariable < out.m_variable ) {
+				out.m_contactFlag = true;
+				out.m_variable = tmpVariable;
+				out.m_contactPoint = ray.m_startPosition + tmpVariable * ray.m_direction;
+				out.m_contactNormal = transform.getUpper3x3() * tmpNormal;
+				subData.setIslandId(i);
+				out.m_subData = subData;
+				ret = true;
+			}
+
+		}
+	}
+
+	return ret;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_large_tri_mesh.h

@@ -0,0 +1,30 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_RAYLARGETRIMESH_H
+#define _SCE_PFX_INTERSECT_RAYLARGETRIMESH_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRayLargeTriMesh(const PfxRayInput &ray,PfxRayOutput &out,const void *shape,const PfxTransform3 &transform);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_INTERSECT_RAYLARGETRIMESH_H

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_sphere.cpp

@@ -0,0 +1,55 @@
+/*
+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/collision/pfx_sphere.h"
+#include "pfx_intersect_common.h"
+#include "pfx_intersect_ray_sphere.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRaySphere(const PfxRayInput &ray,PfxRayOutput &out,const PfxSphere &sphere,const PfxTransform3 &transform)
+{
+	PfxVector3 v = ray.m_startPosition - transform.getTranslation();
+
+	PfxFloat a = dot(ray.m_direction,ray.m_direction);
+	PfxFloat b = dot(v,ray.m_direction);
+	PfxFloat c = dot(v,v) - sphere.m_radius * sphere.m_radius;
+
+	if(c < 0.0f) return false;
+
+	PfxFloat d = b * b - a * c;
+	
+	if(d < 0.0f || fabsf(a) < 0.00001f) return false;
+	
+	PfxFloat tt = ( -b - sqrtf(d) ) / a;
+	
+	if(tt < 0.0f || tt > 1.0f) return false;
+	
+	if(tt < out.m_variable) {
+		out.m_contactFlag = true;
+		out.m_variable = tt;
+		out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
+		out.m_contactNormal = normalize(out.m_contactPoint - transform.getTranslation());
+		out.m_subData.m_type = PfxSubData::NONE;
+		return true;
+	}
+	
+	return false;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_sphere.h

@@ -0,0 +1,31 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_RAYSPHERE_H
+#define _SCE_PFX_INTERSECT_RAYSPHERE_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_sphere.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxBool pfxIntersectRaySphere(const PfxRayInput &ray,PfxRayOutput &out,const PfxSphere &sphere,const PfxTransform3 &transform);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_INTERSECT_RAYSPHERE_H

Extras/PhysicsEffects/src/base_level/collision/pfx_mesh_common.h

@@ -0,0 +1,194 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_MESH_COMMON_H
+#define _SCE_PFX_MESH_COMMON_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+struct PfxClosestPoints {
+	PfxPoint3 pA[4],pB[4];
+	PfxFloat distSqr[4];
+	PfxFloat closestDistSqr;
+	int numPoints;
+	SCE_PFX_PADDING(1,8)
+	
+	PfxClosestPoints()
+	{
+		numPoints = 0;
+		closestDistSqr = SCE_PFX_FLT_MAX;
+	}
+	
+	void set(int i,const PfxPoint3 &pointA,const PfxPoint3 &pointB,PfxFloat d)
+	{
+		pA[i] = pointA;
+		pB[i] = pointB;
+		distSqr[i] = d;
+	}
+	
+	void add(const PfxPoint3 &pointA,const PfxPoint3 &pointB,PfxFloat d)
+	{
+		const PfxFloat epsilon = 0.00001f;
+		if(closestDistSqr < d) return;
+		
+		closestDistSqr = d + epsilon;
+		
+		int replaceId = -1;
+		PfxFloat distMax = -SCE_PFX_FLT_MAX;
+		for(int i=0;i<numPoints;i++) {
+			if(lengthSqr(pA[i]-pointA) < epsilon) {
+				return;
+			}
+			if(distMax < distSqr[i]) {
+				distMax = distSqr[i];
+				replaceId = i;
+			}
+		}
+		
+		replaceId = (numPoints<4)?(numPoints++):replaceId;
+		
+		set(replaceId,pointA,pointB,d);
+	}
+};
+
+static SCE_PFX_FORCE_INLINE
+PfxUInt32 pfxGatherFacets(
+	const PfxTriMesh *mesh,
+	const PfxFloat *aabbHalf,
+	const PfxVector3 &offsetPos,
+	const PfxMatrix3 &offsetRot,
+	PfxUInt8 *selFacets)
+{
+	PfxMatrix3 absOffsetRot = absPerElem(offsetRot);
+	
+	PfxUInt32 numSelFacets = 0;
+	
+for(int f=0;f<(int)mesh->m_numFacets;f++) {
+	const PfxFacet &facet = mesh->m_facets[f];
+
+	PfxVector3 facetCenter = absPerElem(offsetPos + offsetRot * pfxReadVector3(facet.m_center));
+	PfxVector3 halfBA = absOffsetRot * pfxReadVector3(facet.m_half);
+
+	// ConvexBのAABBとチェック
+	if(facetCenter[0] > (halfBA[0]+aabbHalf[0])) continue;
+	if(facetCenter[1] > (halfBA[1]+aabbHalf[1])) continue;
+	if(facetCenter[2] > (halfBA[2]+aabbHalf[2])) continue;
+	
+	// この面は判定
+	selFacets[numSelFacets++] = (PfxUInt8)f;
+}
+	
+	return numSelFacets;
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxGetProjAxisPnts6(
+	const PfxVector3 *verts,const PfxVector3 &axis,
+	PfxFloat &distMin,PfxFloat &distMax)
+{
+PfxFloat p0 = dot(axis, verts[0]);
+PfxFloat p1 = dot(axis, verts[1]);
+PfxFloat p2 = dot(axis, verts[2]);
+PfxFloat p3 = dot(axis, verts[3]);
+PfxFloat p4 = dot(axis, verts[4]);
+PfxFloat p5 = dot(axis, verts[5]);
+distMin = SCE_PFX_MIN(p5,SCE_PFX_MIN(p4,SCE_PFX_MIN(p3,SCE_PFX_MIN(p2,SCE_PFX_MIN(p0,p1)))));
+distMax = SCE_PFX_MAX(p5,SCE_PFX_MAX(p4,SCE_PFX_MAX(p3,SCE_PFX_MAX(p2,SCE_PFX_MAX(p0,p1)))));
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxGetProjAxisPnts3(
+	const PfxVector3 *verts,const PfxVector3 &axis,
+	PfxFloat &distMin,PfxFloat &distMax)
+{
+PfxFloat p0 = dot(axis, verts[0]);
+PfxFloat p1 = dot(axis, verts[1]);
+PfxFloat p2 = dot(axis, verts[2]);
+distMin = SCE_PFX_MIN(p2,SCE_PFX_MIN(p0,p1));
+distMax = SCE_PFX_MAX(p2,SCE_PFX_MAX(p0,p1));
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxGetProjAxisPnts2(
+	const PfxVector3 *verts,const PfxVector3 &axis,
+	PfxFloat &distMin,PfxFloat &distMax)
+{
+PfxFloat p0 = dot(axis, verts[0]);
+PfxFloat p1 = dot(axis, verts[1]);
+distMin = SCE_PFX_MIN(p0,p1);
+distMax = SCE_PFX_MAX(p0,p1);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// ２つのベクトルの向きをチェック
+
+static SCE_PFX_FORCE_INLINE
+bool pfxIsSameDirection(const PfxVector3 &vecA,const PfxVector3 &vecB)
+{
+return fabsf(dot(vecA,vecB)) > 0.9999f;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// 面ローカルの座標を算出
+
+static SCE_PFX_FORCE_INLINE
+void pfxGetLocalCoords(
+	const PfxVector3 &pointOnTriangle,
+	const PfxTriangle &triangle,
+	PfxFloat &s,PfxFloat &t)
+{
+	PfxVector3 v0 = triangle.points[1] - triangle.points[0];
+	PfxVector3 v1 = triangle.points[2] - triangle.points[0];
+	PfxVector3 dir = pointOnTriangle - triangle.points[0];
+	PfxVector3 v = cross( v0, v1 );
+	PfxVector3 crS = cross( v, v0 );
+	PfxVector3 crT = cross( v, v1 );
+	s = dot( crT, dir ) / dot( crT, v0 );
+	t = dot( crS, dir ) / dot( crS, v1 );
+}
+
+// a,bからなる直線上に点pがあるかどうかを判定
+static SCE_PFX_FORCE_INLINE
+bool pfxPointOnLine(const PfxVector3 &p,const PfxVector3 &a,const PfxVector3 &b)
+{
+	PfxVector3 ab = normalize(b-a);
+	PfxVector3 q = a + ab * dot(p-a,ab);
+	return lengthSqr(p-q) < 0.00001f;
+}
+
+// 線分a,b上に点pがあるかどうかを判定
+static SCE_PFX_FORCE_INLINE
+bool pfxPointOnSegment(const PfxVector3 &p,const PfxVector3 &a,const PfxVector3 &b)
+{
+	PfxVector3 ab = b-a;
+	PfxVector3 ap = p-a;
+	PfxFloat denom = dot(ab,ab);
+	PfxFloat num = dot(ap,ab);
+	PfxFloat t = num/denom;
+	if(t < 0.0f || t > 1.0f) return false;
+	return (dot(ap,ap)-num*t) < 0.00001f;
+}
+
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_MESH_COMMON_H

Extras/PhysicsEffects/src/base_level/collision/pfx_shape.cpp

@@ -0,0 +1,95 @@
+/*
+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/collision/pfx_shape.h"
+
+namespace sce {
+namespace PhysicsEffects {
+void pfxGetShapeAabbDummy(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
+{
+	(void)shape,(void)aabbMin,(void)aabbMax;
+}
+
+void pfxGetShapeAabbSphere(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
+{
+	aabbMin = shape.getOffsetPosition() - PfxVector3(shape.getSphere().m_radius);
+	aabbMax = shape.getOffsetPosition() + PfxVector3(shape.getSphere().m_radius);
+}
+
+void pfxGetShapeAabbBox(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
+{
+	PfxVector3 boxSize = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * shape.getBox().m_half;
+	aabbMin = shape.getOffsetPosition() - boxSize;
+	aabbMax = shape.getOffsetPosition() + boxSize;
+}
+
+void pfxGetShapeAabbCapsule(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
+{
+	PfxVector3 dir = rotate(shape.getOffsetOrientation(),PfxVector3(1.0f,0.0f,0.0f));
+	PfxVector3 capSize = absPerElem(dir) * shape.getCapsule().m_halfLen + 
+		PfxVector3(shape.getCapsule().m_radius);
+	aabbMin = shape.getOffsetPosition() - capSize;
+	aabbMax = shape.getOffsetPosition() + capSize;
+}
+
+void pfxGetShapeAabbCylinder(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
+{
+	PfxVector3 capSize = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * 
+		PfxVector3(shape.getCylinder().m_halfLen,shape.getCylinder().m_radius,shape.getCylinder().m_radius);
+	aabbMin = shape.getOffsetPosition() - capSize;
+	aabbMax = shape.getOffsetPosition() + capSize;
+}
+
+void pfxGetShapeAabbConvexMesh(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
+{
+	const PfxConvexMesh *convex = shape.getConvexMesh();
+	PfxVector3 half = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * convex->m_half;
+	aabbMin = shape.getOffsetPosition() - half;
+	aabbMax = shape.getOffsetPosition() + half;
+}
+
+void pfxGetShapeAabbLargeTriMesh(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
+{
+	const PfxLargeTriMesh *largemesh = shape.getLargeTriMesh();
+	PfxVector3 half = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) *  largemesh->m_half;
+	aabbMin = shape.getOffsetPosition() - half;
+	aabbMax = shape.getOffsetPosition() + half;
+}
+
+typedef void (*PfxFuncGetShapeAabb)(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax);
+
+PfxFuncGetShapeAabb pfxFuncGetShapeAabb[kPfxShapeCount] = {
+	pfxGetShapeAabbSphere,
+	pfxGetShapeAabbBox,
+	pfxGetShapeAabbCapsule,
+	pfxGetShapeAabbCylinder,
+	pfxGetShapeAabbConvexMesh,
+	pfxGetShapeAabbLargeTriMesh,
+	pfxGetShapeAabbDummy,
+	pfxGetShapeAabbDummy,
+	pfxGetShapeAabbDummy,
+	pfxGetShapeAabbDummy,
+	pfxGetShapeAabbDummy,
+	pfxGetShapeAabbDummy,
+};
+
+void PfxShape::getAabb(PfxVector3 &aabbMin,PfxVector3 &aabbMax) const
+{
+	return pfxFuncGetShapeAabb[m_type](*this,aabbMin,aabbMax);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_simplex_solver.cpp

@@ -0,0 +1,284 @@
+/*
+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 "pfx_simplex_solver.h"
+#include "pfx_gjk_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+inline static
+bool operator ==(const PfxVector3 &a,const PfxVector3 &b)
+{
+	return lengthSqr(a-b) < (SCE_PFX_GJK_EPSILON * SCE_PFX_GJK_EPSILON);
+}
+
+bool PfxSimplexSolver::closest(PfxVector3& v)
+{
+	bool ret = false;
+
+	bc.reset();
+
+	switch(numVertices) {
+		case 0:
+		ret = false;
+		break;
+
+		case 1:
+		{
+			PfxVector3 tmpP = P[0];
+			PfxVector3 tmpQ = Q[0];
+			v = tmpP-tmpQ;
+			bc.reset();
+			bc.setBarycentricCoordinates(1.0f,0.0f,0.0f,0.0f);
+			ret = bc.isValid();
+		}
+		break;
+
+		case 2:
+		{
+			PfxVector3 dir = W[1] - W[0];
+			PfxFloat t = dot(-W[0],dir) / dot(dir,dir);
+
+			if(t < 0.0f) t = 0.0f;
+			if(t > 1.0f) t = 1.0f;
+
+			bc.setBarycentricCoordinates(1-t,t,0.0f,0.0f);
+
+			PfxVector3 tmpP = P[0] + t * (P[1] - P[0]);
+			PfxVector3 tmpQ = Q[0] + t * (Q[1] - Q[0]);
+			v = tmpP - tmpQ;
+
+			reduceVertices();
+
+			ret = bc.isValid();
+			break;
+		}
+
+		case 3: 
+		{ 
+			const PfxVector3& a = W[0]; 
+			const PfxVector3& b = W[1]; 
+			const PfxVector3& c = W[2]; 
+
+			closestPointTriangleFromOrigin(a,b,c,bc);
+
+		PfxVector3 tmpP = P[0] * bc.barycentricCoords[0] + 
+					   P[1] * bc.barycentricCoords[1] + 
+					   P[2] * bc.barycentricCoords[2]; 
+
+		PfxVector3 tmpQ = Q[0] * bc.barycentricCoords[0] + 
+					   Q[1] * bc.barycentricCoords[1] + 
+					   Q[2] * bc.barycentricCoords[2]; 
+
+			v = tmpP-tmpQ; 
+
+			reduceVertices(); 
+			ret = bc.isValid(); 
+			break; 
+		}
+
+		case 4:
+		{
+			const PfxVector3& a = W[0];
+			const PfxVector3& b = W[1];
+			const PfxVector3& c = W[2];
+			const PfxVector3& d = W[3];
+
+			if(closestPointTetrahedronFromOrigin(a,b,c,d,bc)) {
+			PfxVector3 tmpP = P[0] * bc.barycentricCoords[0] +
+						   P[1] * bc.barycentricCoords[1] +
+						   P[2] * bc.barycentricCoords[2] +
+						   P[3] * bc.barycentricCoords[3];
+
+			PfxVector3 tmpQ = Q[0] * bc.barycentricCoords[0] +
+						   Q[1] * bc.barycentricCoords[1] +
+						   Q[2] * bc.barycentricCoords[2] +
+						   Q[3] * bc.barycentricCoords[3];
+				v = tmpP-tmpQ;
+
+				reduceVertices();
+				ret = bc.isValid();
+			} else {
+				// 原点が内部に存在→交差している
+				ret = true;
+				v = PfxVector3(0.0f);
+			}
+			break;
+		}
+	};
+
+	return ret;
+}
+
+bool PfxSimplexSolver::inSimplex(const PfxVector3& w)
+{
+	for(int i=0;i<numVertices;i++) {
+		if(W[i] == w)
+			return true;
+	}
+	return false;
+}
+
+bool PfxSimplexSolver::closestPointTriangleFromOrigin(const PfxVector3 &a,const  PfxVector3 &b,const  PfxVector3 &c,PfxBarycentricCoords& result)
+{
+	result.usedVertices = 0;
+	PfxVector3 p(0.0f);
+
+    PfxVector3 ab = b - a;
+    PfxVector3 ac = c - a;
+    PfxVector3 ap = p - a;
+    PfxFloat d1 = dot(ab,ap);
+    PfxFloat d2 = dot(ac,ap);
+    if(d1 <= 0.0f && d2 <= 0.0f) {
+		result.closest = a;
+		result.setBarycentricCoordinates(1.0f,0.0f,0.0f,0.0f);
+		return true;
+	}
+
+    PfxVector3 bp = p - b;
+    PfxFloat d3 = dot(ab,bp);
+    PfxFloat d4 = dot(ac,bp);
+    if(d3 >= 0.0f && d4 <= d3) {
+		result.closest = b;
+		result.setBarycentricCoordinates(0.0f,1.0f,0.0f,0.0f);
+		return true;
+	}
+
+    PfxFloat vc = d1*d4 - d3*d2;
+    if(vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
+        PfxFloat v = d1 / (d1 - d3);
+		result.closest = a + v * ab;
+		result.setBarycentricCoordinates(1.0f-v,v,0.0f,0.0f);
+		return true;
+    }
+
+    PfxVector3 cp = p - c;
+    PfxFloat d5 = dot(ab,cp);
+    PfxFloat d6 = dot(ac,cp);
+    if(d6 >= 0.0f && d5 <= d6) {
+		result.closest = c;
+		result.setBarycentricCoordinates(0.0f,0.0f,1.0f,0.0f);
+		return true;
+	}
+
+    PfxFloat vb = d5*d2 - d1*d6;
+    if(vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
+        PfxFloat w = d2 / (d2 - d6);
+		result.closest = a + w * ac;
+		result.setBarycentricCoordinates(1.0f-w,0.0f,w,0.0f);
+		return true;
+    }
+
+    PfxFloat va = d3*d6 - d5*d4;
+    if(va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
+        PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		result.closest = b + w * (c - b);
+		result.setBarycentricCoordinates(0.0f,1.0f-w,w,0.0f);
+		return true;		
+    }
+
+    PfxFloat denom = PfxFloat(1.0) / (va + vb + vc);
+    PfxFloat v = vb * denom;
+    PfxFloat w = vc * denom;
+    
+	result.closest = a + ab * v + ac * w;
+	result.setBarycentricCoordinates(1.0f-v-w,v,w,0.0f);
+	
+	return true;
+}
+
+bool PfxSimplexSolver::closestPointTetrahedronFromOrigin(const PfxVector3 &a,const  PfxVector3 &b,const  PfxVector3 &c,const  PfxVector3 &d,PfxBarycentricCoords& finalResult)
+{
+	PfxBarycentricCoords tempResult;
+	PfxVector3 p(0.0f);
+
+	finalResult.closest = p;
+	finalResult.usedVertices = 0;
+
+	bool pointOutsideABC = originOutsideOfPlane(a, b, c, d);
+	bool pointOutsideACD = originOutsideOfPlane(a, c, d, b);
+	bool pointOutsideADB = originOutsideOfPlane(a, d, b, c);
+	bool pointOutsideBDC = originOutsideOfPlane(b, d, c, a);
+
+	if(!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+		return false;
+
+	PfxFloat bestSqDist = SCE_PFX_FLT_MAX;
+
+	if(pointOutsideABC) {
+		closestPointTriangleFromOrigin(a, b, c,tempResult);
+		PfxVector3 q = tempResult.closest;
+		PfxFloat sqDist = dot((q - p),(q - p));
+		if(sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.closest = q;
+			finalResult.setBarycentricCoordinates(
+					tempResult.barycentricCoords[0],
+					tempResult.barycentricCoords[1],
+					tempResult.barycentricCoords[2],
+					0);
+		}
+    }
+  
+	if(pointOutsideACD) {
+		closestPointTriangleFromOrigin(a, c, d,tempResult);
+		PfxVector3 q = tempResult.closest;
+        PfxFloat sqDist = dot((q - p),(q - p));
+        if(sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.closest = q;
+			finalResult.setBarycentricCoordinates(
+					tempResult.barycentricCoords[0],
+					0,
+					tempResult.barycentricCoords[1],
+					tempResult.barycentricCoords[2]);
+		}
+    }
+	
+	if(pointOutsideADB) {
+		closestPointTriangleFromOrigin(a, d, b,tempResult);
+		PfxVector3 q = tempResult.closest;
+        PfxFloat sqDist = dot((q - p),(q - p));
+        if(sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.closest = q;
+			finalResult.setBarycentricCoordinates(
+					tempResult.barycentricCoords[0],
+					tempResult.barycentricCoords[2],
+					0,
+					tempResult.barycentricCoords[1]);
+		}
+    }
+
+	if(pointOutsideBDC) {
+		closestPointTriangleFromOrigin(b, d, c,tempResult);
+		PfxVector3 q = tempResult.closest;
+        PfxFloat sqDist = dot((q - p),(q - p));
+        if(sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.closest = q;
+			finalResult.setBarycentricCoordinates(
+					0,
+					tempResult.barycentricCoords[0],
+					tempResult.barycentricCoords[2],
+					tempResult.barycentricCoords[1]);
+		}
+    }
+
+    return true;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/collision/pfx_simplex_solver.h

@@ -0,0 +1,153 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_SIMPLEX_SOLVER_H
+#define _SCE_PFX_SIMPLEX_SOLVER_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Voronoi Simplex Solver
+
+struct SCE_PFX_ALIGNED(16) PfxBarycentricCoords {
+	PfxVector3 closest;
+PfxFloat barycentricCoords[4];
+	unsigned int usedVertices;
+	SCE_PFX_PADDING(1,12)
+
+	void reset()
+	{
+	barycentricCoords[0] = 0.0f;
+	barycentricCoords[1] = 0.0f;
+	barycentricCoords[2] = 0.0f;
+	barycentricCoords[3] = 0.0f;
+		usedVertices = 0;
+	}
+
+	bool isValid()
+	{
+	return   (barycentricCoords[0] >= 0.0f) &&
+			 (barycentricCoords[1] >= 0.0f) &&
+			 (barycentricCoords[2] >= 0.0f) &&
+			 (barycentricCoords[3] >= 0.0f);
+	}
+
+void setBarycentricCoordinates(PfxFloat a,PfxFloat b,PfxFloat c,PfxFloat d)
+{
+	barycentricCoords[0] = a;
+	barycentricCoords[1] = b;
+	barycentricCoords[2] = c;
+	barycentricCoords[3] = d;
+	if(a != 0.0f) usedVertices |= 1<<3;
+	if(b != 0.0f) usedVertices |= 1<<2;
+	if(c != 0.0f) usedVertices |= 1<<1;
+	if(d != 0.0f) usedVertices |= 1;
+}
+};
+
+class PfxSimplexSolver {
+private:
+	 static const int MAX_VERTS = 4;
+
+public:
+	int	numVertices;
+	SCE_PFX_PADDING(1,12)
+	PfxVector3	W[MAX_VERTS];
+	PfxVector3	P[MAX_VERTS];
+	PfxVector3	Q[MAX_VERTS];
+
+	PfxBarycentricCoords bc;
+
+	inline void	removeVertex(int index);
+	inline void	reduceVertices ();
+
+	inline bool	originOutsideOfPlane(const PfxVector3& a, const PfxVector3& b, const PfxVector3& c, const PfxVector3& d);
+	bool	closestPointTetrahedronFromOrigin(const PfxVector3 &a,const  PfxVector3 &b,const  PfxVector3 &c,const  PfxVector3 &d, PfxBarycentricCoords& result);
+	bool	closestPointTriangleFromOrigin(const PfxVector3 &a,const  PfxVector3 &b,const  PfxVector3 &c,PfxBarycentricCoords& result);
+
+public:
+	void reset()
+	{
+		numVertices = 0;
+		bc.reset();
+	}
+
+	inline void addVertex(const PfxVector3& w_, const PfxVector3& p_, const PfxVector3& q_);
+
+	bool closest(PfxVector3& v);
+
+	bool fullSimplex() const
+	{
+		return (numVertices == 4);
+	}
+
+	bool inSimplex(const PfxVector3& w);
+};
+
+inline
+void	PfxSimplexSolver::removeVertex(int index)
+{
+	SCE_PFX_ASSERT(numVertices>0);
+	numVertices--;
+	W[index] = W[numVertices];
+	P[index] = P[numVertices];
+	Q[index] = Q[numVertices];
+}
+
+inline
+void	PfxSimplexSolver::reduceVertices ()
+{
+	if ((numVertices >= 4) && (!(bc.usedVertices&0x01)))
+		removeVertex(3);
+
+	if ((numVertices >= 3) && (!(bc.usedVertices&0x02)))
+		removeVertex(2);
+
+	if ((numVertices >= 2) && (!(bc.usedVertices&0x04)))
+		removeVertex(1);
+	
+	if ((numVertices >= 1) && (!(bc.usedVertices&0x08)))
+		removeVertex(0);
+}
+
+inline
+void PfxSimplexSolver::addVertex(const PfxVector3& w, const PfxVector3& p, const PfxVector3& q)
+{
+	W[numVertices] = w;
+	P[numVertices] = p;
+	Q[numVertices] = q;
+	numVertices++;
+}
+
+inline
+bool PfxSimplexSolver::originOutsideOfPlane(const PfxVector3& a, const PfxVector3& b, const PfxVector3& c, const PfxVector3& d)
+{
+	PfxVector3 normal = cross((b-a),(c-a));
+
+    PfxFloat signp = dot(-a,normal);
+    PfxFloat signd = dot((d - a),normal);
+
+	return signp * signd < 0.0f;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_SIMPLEX_SOLVER_H

Extras/PhysicsEffects/src/base_level/premake4.lua

@@ -0,0 +1,12 @@
+	project "physicseffects2_baselevel"
+		
+	kind "StaticLib"
+	targetdir "../../build/lib"	
+	includedirs {
+		".",
+	}
+	files {
+		"**.cpp",
+		"../../include/physics_effects/base_level/**.h"
+
+	}

Extras/PhysicsEffects/src/base_level/solver/pfx_check_solver.h

@@ -0,0 +1,67 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CHECK_SOLVER_H_
+#define _SCE_PFX_CHECK_SOLVER_H_
+
+#include "../../../include/physics_effects/base_level/rigidbody/pfx_rigid_state.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_constraint_pair.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+// Solver check table
+/*
+	-----------------MotionTypeA
+	|0	1	0	1	0
+	|1	1	1	1	0
+	|0	1	0	1	0
+	|1	1	1	0	0
+	|0	0	0	0	0
+MotionTypeB
+ */
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxCheckSolverTable(ePfxMotionType i,ePfxMotionType j)
+{
+	const PfxUInt32 solverTable = 0x00af2b80;
+
+	SCE_PFX_ASSERT(i < kPfxMotionTypeCount);
+	SCE_PFX_ASSERT(j < kPfxMotionTypeCount);
+
+	PfxUInt32 idx = j * kPfxMotionTypeCount + i;
+	PfxUInt32 mask = 1 << (kPfxMotionTypeCount*kPfxMotionTypeCount-1-idx);
+	return (solverTable & mask) != 0;
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxBool pfxCheckSolver(const PfxConstraintPair &pair)
+{
+	PfxUInt32 motionA = pfxGetMotionMaskA(pair)&SCE_PFX_MOTION_MASK_TYPE;
+	PfxUInt32 motionB = pfxGetMotionMaskB(pair)&SCE_PFX_MOTION_MASK_TYPE;
+	PfxUInt32 sleepA = pfxGetMotionMaskA(pair)&SCE_PFX_MOTION_MASK_SLEEPING;
+	PfxUInt32 sleepB = pfxGetMotionMaskB(pair)&SCE_PFX_MOTION_MASK_SLEEPING;
+	
+	return
+		pfxGetActive(pair) &&
+		pfxCheckSolverTable((ePfxMotionType)motionA,(ePfxMotionType)motionB) && // モーションタイプ別衝突判定テーブル
+		!((sleepA != 0 && sleepB != 0) || (sleepA != 0 && motionB == kPfxMotionTypeFixed) || (sleepB != 0 && motionA == kPfxMotionTypeFixed));// スリープ時のチェック
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif /* _SCE_PFX_CHECK_SOLVER_H_ */

Extras/PhysicsEffects/src/base_level/solver/pfx_constraint_row_solver.h

@@ -0,0 +1,296 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_CONSTRAINT_ROW_SOLVER_H
+#define _SCE_PFX_CONSTRAINT_ROW_SOLVER_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_constraint_row.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_joint_constraint.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Constraint Row Solver
+
+// ARA begin insert new code
+#ifdef __ARM_NEON__
+// prototype for inline NEON assembly version
+void pfxSolveLinearConstraintRowNEON(PfxConstraintRow &constraint,
+	PfxVector3 &deltaLinearVelocityA,PfxVector3 &deltaAngularVelocityA,
+	PfxFloat &massInvA,const PfxMatrix3 &inertiaInvA,const PfxVector3 &rA,
+	PfxVector3 &deltaLinearVelocityB,PfxVector3 &deltaAngularVelocityB,
+	PfxFloat &massInvB,const PfxMatrix3 &inertiaInvB,const PfxVector3 &rB);
+#endif // __ARM_NEON__
+// ARA end
+
+static SCE_PFX_FORCE_INLINE
+void pfxSolveLinearConstraintRow(PfxConstraintRow &constraint,
+	PfxVector3 &deltaLinearVelocityA,PfxVector3 &deltaAngularVelocityA,
+	PfxFloat massInvA,const PfxMatrix3 &inertiaInvA,const PfxVector3 &rA,
+	PfxVector3 &deltaLinearVelocityB,PfxVector3 &deltaAngularVelocityB,
+	PfxFloat massInvB,const PfxMatrix3 &inertiaInvB,const PfxVector3 &rB)
+{
+const PfxVector3 normal(pfxReadVector3(constraint.m_normal));
+PfxFloat deltaImpulse = constraint.m_rhs;
+PfxVector3 dVA = deltaLinearVelocityA + cross(deltaAngularVelocityA,rA);
+PfxVector3 dVB = deltaLinearVelocityB + cross(deltaAngularVelocityB,rB);
+deltaImpulse -= constraint.m_jacDiagInv * dot(normal,dVA-dVB);
+PfxFloat oldImpulse = constraint.m_accumImpulse;
+constraint.m_accumImpulse = SCE_PFX_CLAMP(oldImpulse + deltaImpulse,constraint.m_lowerLimit,constraint.m_upperLimit);
+deltaImpulse = constraint.m_accumImpulse - oldImpulse;
+deltaLinearVelocityA += deltaImpulse * massInvA * normal;
+deltaAngularVelocityA += deltaImpulse * inertiaInvA * cross(rA,normal);
+deltaLinearVelocityB -= deltaImpulse * massInvB * normal;
+deltaAngularVelocityB -= deltaImpulse * inertiaInvB * cross(rB,normal);
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxSolveAngularConstraintRow(PfxConstraintRow &constraint,
+	PfxVector3 &deltaAngularVelocityA,
+	const PfxMatrix3 &inertiaInvA,const PfxVector3 &rA,
+	PfxVector3 &deltaAngularVelocityB,
+	const PfxMatrix3 &inertiaInvB,const PfxVector3 &rB)
+{
+	(void)rA,(void)rB;
+const PfxVector3 normal(pfxReadVector3(constraint.m_normal));
+PfxFloat deltaImpulse = constraint.m_rhs;
+deltaImpulse -= constraint.m_jacDiagInv * dot(normal,deltaAngularVelocityA-deltaAngularVelocityB);
+PfxFloat oldImpulse = constraint.m_accumImpulse;
+constraint.m_accumImpulse = SCE_PFX_CLAMP(oldImpulse + deltaImpulse,constraint.m_lowerLimit,constraint.m_upperLimit);
+deltaImpulse = constraint.m_accumImpulse - oldImpulse;
+deltaAngularVelocityA += deltaImpulse * inertiaInvA * normal;
+deltaAngularVelocityB -= deltaImpulse * inertiaInvB * normal;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Calc Joint Angle
+
+static SCE_PFX_FORCE_INLINE
+void pfxCalcJointAngleSwingTwist(PfxMatrix3 &worldFrameA,PfxMatrix3 &worldFrameB,PfxFloat *angle,PfxVector3 *axis)
+{
+	// フレームA座標系への変換マトリクス
+	PfxMatrix3 frameBA = transpose(worldFrameA) * worldFrameB;
+
+	// クォータニオン回転をtwistとswingに分離
+	PfxQuat swing,twist,qBA(frameBA);
+	swing = PfxQuat::rotation(PfxVector3(1.0f,0.0f,0.0f),frameBA.getCol0());
+	twist = qBA * conj(swing);
+
+	if(dot(twist,PfxQuat::rotationX(0.0f)) < 0.0f) {
+		twist = -twist;
+	}
+
+	// それぞれの回転軸と回転角度を算出
+	pfxGetRotationAngleAndAxis(normalize(twist),angle[0],axis[0]);
+	pfxGetRotationAngleAndAxis(normalize(swing),angle[1],axis[1]);
+
+	if(angle[1] < 0.00001f) {
+		axis[1] = PfxVector3(0.0f,1.0f,0.0f);
+	}
+
+	// twistの軸方向のチェック
+	if(dot(axis[0],frameBA.getCol0()) < 0.0f) {
+		angle[0] = -angle[0];
+	}
+
+	axis[0] = worldFrameB.getCol0();
+	axis[1] = worldFrameA * axis[1];
+	axis[2] = cross(axis[0],axis[1]);
+	angle[2] = 0.0f;
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxCalcJointAngleSwing1Swing2Twist(PfxMatrix3 &worldFrameA,PfxMatrix3 &worldFrameB,PfxFloat *angle,PfxVector3 *axis)
+{
+	// フレームA座標系への変換マトリクス
+	PfxMatrix3 frameBA = transpose(worldFrameA) * worldFrameB;
+
+	// クォータニオン回転をtwistとswingに分離
+	PfxQuat swing,twist,qBA(frameBA);
+	swing = PfxQuat::rotation(PfxVector3(1.0f,0.0f,0.0f),frameBA.getCol0());
+	twist = qBA * conj(swing);
+
+	if(dot(twist,PfxQuat::rotationX(0.0f)) < 0.0f) {
+		twist = -twist;
+	}
+
+	PfxQuat swing1,swing2;
+
+	PfxVector3 pXY = frameBA.getCol0();pXY[2] = 0.0f;
+	PfxVector3 pXZ = frameBA.getCol0();pXZ[1] = 0.0f;
+
+	if(fabsf(frameBA.getCol0()[1]) < fabsf(frameBA.getCol0()[2])) {
+		swing1 = PfxQuat::rotation(PfxVector3(1.0f,0.0f,0.0f),pXZ);
+		swing2 = swing * conj(swing1);
+	}
+	else {
+		swing2 = PfxQuat::rotation(PfxVector3(1.0f,0.0f,0.0f),pXY);
+		swing1 = conj(swing2) * swing;
+	}
+
+	// それぞれの回転軸と回転角度を算出
+	pfxGetRotationAngleAndAxis(normalize(twist),angle[0],axis[0]);
+	pfxGetRotationAngleAndAxis(normalize(swing2),angle[1],axis[1]);
+	pfxGetRotationAngleAndAxis(normalize(swing1),angle[2],axis[2]);
+
+	if(angle[1] < 0.00001f) {
+		angle[1] = 0.0f;
+		axis[1] = PfxVector3(0.0f,1.0f,0.0f);
+	}
+
+	if(angle[2] < 0.00001f) {
+		angle[2] = 0.0f;
+		axis[2] = PfxVector3(0.0f,0.0f,1.0f);
+	}
+
+	// twistの軸方向のチェック
+	if(dot(axis[0],frameBA.getCol0()) < 0.0f) {
+		angle[0] = -angle[0];
+	}
+
+	axis[0] = worldFrameB.getCol0();
+	axis[1] = worldFrameA * axis[1];
+	axis[2] = worldFrameA * axis[2];
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxCalcJointAngleUniversal(PfxMatrix3 &worldFrameA,PfxMatrix3 &worldFrameB,PfxFloat *angle,PfxVector3 *axis)
+{
+	// フレームA座標系への変換マトリクス
+	PfxMatrix3 frameBA = transpose(worldFrameA) * worldFrameB;
+
+	// クォータニオン回転をtwistとswingに分離
+	PfxQuat swing,swing1,swing2,twist,qBA(frameBA);
+	PfxVector3 Pxy(frameBA.getCol0());
+	Pxy[2] = 0.0f;
+	swing1 = PfxQuat::rotation(PfxVector3(1.0f,0.0f,0.0f),normalize(Pxy));
+	swing = PfxQuat::rotation(PfxVector3(1.0f,0.0f,0.0f),frameBA.getCol0());
+	swing2 = swing * conj(swing1);
+	twist = conj(swing) * qBA;
+
+	if(dot(twist,PfxQuat::rotationX(0.0f)) < 0.0f) {
+		twist = -twist;
+	}
+
+	pfxGetRotationAngleAndAxis(normalize(twist),angle[0],axis[0]);
+	pfxGetRotationAngleAndAxis(normalize(swing1),angle[1],axis[1]);
+	pfxGetRotationAngleAndAxis(normalize(swing2),angle[2],axis[2]);
+
+	if(axis[1].getZ() < 0.0f) {
+		axis[1] = -axis[1];
+		angle[1] = -angle[1];
+	}
+
+	PfxVector3 chkY = cross(PfxVector3(0.0f,0.0f,1.0f),frameBA.getCol0());
+	if(dot(chkY,axis[2]) < 0.0f) {
+		axis[2] = -axis[2];
+		angle[2] = -angle[2];
+	}
+
+	// twistの軸方向のチェック
+	if(dot(axis[0],frameBA.getCol0()) < 0.0f) {
+		angle[0] = -angle[0];
+	}
+
+	axis[0] = worldFrameB.getCol0();
+	axis[1] = worldFrameA * axis[1];
+	axis[2] = worldFrameA * axis[2];
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Calc Joint Limit
+
+static SCE_PFX_FORCE_INLINE
+void pfxCalcLinearLimit(
+	const PfxJointConstraint &jointConstraint,
+	PfxFloat &posErr,PfxFloat &velocityAmp,PfxFloat &lowerLimit,PfxFloat &upperLimit)
+{
+	switch(jointConstraint.m_lock) {
+		case SCE_PFX_JOINT_LOCK_FREE:
+		posErr = 0.0f;
+		velocityAmp *= jointConstraint.m_damping;
+		break;
+		
+		case SCE_PFX_JOINT_LOCK_LIMIT:
+		if(posErr >= jointConstraint.m_movableLowerLimit && posErr <= jointConstraint.m_movableUpperLimit) {
+			posErr = 0.0f;
+			velocityAmp *= jointConstraint.m_damping;
+		}
+		else {
+			if(posErr < jointConstraint.m_movableLowerLimit) {
+				posErr = posErr - jointConstraint.m_movableLowerLimit;
+				posErr = SCE_PFX_MIN(0.0f,posErr+SCE_PFX_JOINT_LINEAR_SLOP);
+				upperLimit = SCE_PFX_MIN(0.0f,upperLimit);
+				velocityAmp = 1.0f;
+			}
+			else { // posErr > movableUpperLimit
+				posErr = posErr - jointConstraint.m_movableUpperLimit;
+				posErr = SCE_PFX_MAX(0.0f,posErr-SCE_PFX_JOINT_LINEAR_SLOP);
+				lowerLimit = SCE_PFX_MAX(0.0f,lowerLimit);
+				velocityAmp = 1.0f;
+			}
+		}
+		break;
+		
+		default: // SCE_PFX_JOINT_LOCK_FIX
+		break;
+	}
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxCalcAngularLimit(
+	const PfxJointConstraint &jointConstraint,
+	PfxFloat &posErr,PfxFloat &velocityAmp,PfxFloat &lowerLimit,PfxFloat &upperLimit)
+{
+	switch(jointConstraint.m_lock) {
+		case SCE_PFX_JOINT_LOCK_FREE:
+		posErr = 0.0f;
+		velocityAmp *= jointConstraint.m_damping;
+		break;
+		
+		case SCE_PFX_JOINT_LOCK_LIMIT:
+		if(posErr >= jointConstraint.m_movableLowerLimit && posErr <= jointConstraint.m_movableUpperLimit) {
+			posErr = 0.0f;
+			velocityAmp *= jointConstraint.m_damping;
+		}
+		else {
+			if(posErr < jointConstraint.m_movableLowerLimit) {
+				posErr = posErr - jointConstraint.m_movableLowerLimit;
+				posErr = SCE_PFX_MIN(0.0f,posErr+SCE_PFX_JOINT_ANGULAR_SLOP);
+				upperLimit = SCE_PFX_MIN(0.0f,upperLimit);
+				velocityAmp = 1.0f;
+			}
+			else { // posErr > movableUpperLimit
+				posErr = posErr - jointConstraint.m_movableUpperLimit;
+				posErr = SCE_PFX_MAX(0.0f,posErr-SCE_PFX_JOINT_ANGULAR_SLOP);
+				lowerLimit = SCE_PFX_MAX(0.0f,lowerLimit);
+				velocityAmp = 1.0f;
+			}
+		}
+		break;
+		
+		default: // SCE_PFX_JOINT_LOCK_FIX
+		break;
+	}
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONSTRAINT_ROW_SOLVER_H

Extras/PhysicsEffects/src/base_level/solver/pfx_constraint_row_solver_neon.cpp

@@ -0,0 +1,186 @@
+/*
+ Applied Research Associates Inc. (c)2011
+
+ Redistribution and use in source and binary forms,
+   with or without modification, are permitted provided that the
+   following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Applied Research Associates Inc nor the names
+      of its contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifdef __ANDROID__
+
+#include "../../../include/physics_effects.h"
+#include <arm_neon.h>
+
+namespace sce {
+namespace PhysicsEffects {
+
+//----------------------------------------------------------------------------
+//  pfxSolveLinearConstraintRowNEON
+//
+/// NEON inline assembly implementation of the pfxSolveLinearConstraintRow
+/// function.
+///
+/// @param  constraint                Constraint row to be solved
+/// @param  deltaLinearVelocityA      Change in linear velocity for object A
+/// @param  deltaAngularVelocityA     Change in angular velocity for object A
+/// @param  massInvA                  Mass inverse for object A
+/// @param  inertiaInvA               Inertia tensor inverse for object A
+/// @param  rA                        Position of contact point for object A
+/// @param  deltaLinearVelocityB      Change in linear velocity for object B
+/// @param  deltaAngularVelocityB     Change in angular velocity for object B
+/// @param  massInvB                  Mass inverse for object B
+/// @param  inertiaInvB               Inertia tensor inverse for object B
+/// @param  rB                        Position of contact point for object B
+//----------------------------------------------------------------------------
+//GSR version
+void pfxSolveLinearConstraintRowNEON(PfxConstraintRow &constraint,
+	PfxVector3 &deltaLinearVelocityA,PfxVector3 &deltaAngularVelocityA,
+	PfxFloat &massInvA,const PfxMatrix3 &inertiaInvA,const PfxVector3 &rA,
+	PfxVector3 &deltaLinearVelocityB,PfxVector3 &deltaAngularVelocityB,
+	PfxFloat &massInvB,const PfxMatrix3 &inertiaInvB,const PfxVector3 &rB)
+{
+	asm volatile
+	(
+		//Loads beforehand so the normal vector will be able to have zero in the 4th element
+		"vld1.32 	{q0}, [%1]			\n\t" //LOAD => deltaLinearVelocityA ----------------> q0
+		"vld1.32	{q1}, [%2]			\n\t" //LOAD => deltaAngularVelocityA ---------------> q1
+		"vld1.32	{q2}, [%6]			\n\t" //LOAD => deltaLinearVelocityB ----------------> q2
+		"vld1.32	{q3}, [%7]			\n\t" //LOAD => deltaAngularVelocityB ---------------> q3
+		//const PfxVector3 normal(pfxReadVector3(constraint.m_normal));
+		"vld1.32	{q4}, [%0]!			\n\t" //LOAD => constraint normal--------------------> q4
+		"vdup.32	d10, d1[1]			\n\t" //set 4th element on normal vector to zero. It is zero since q0 holds a vec3...d1[1] is 4th element and on C++ side we set this to zero
+		"vtrn.32	d9, d10				\n\t" //LOAD => deltaImpulse ------------------------> q5 (d10[0] only)
+		//PfxFloat deltaImpulse = constraint.m_rhs;
+		"vld1.32	{d12}, [%0]!		\n\t" //LOAD => constraint variables ----------------> q6
+		"vld1.32	{d13[0]}, [%0]!		\n\t" //constraint  variables
+		"vld1.32	{d13[1]}, [%0]		\n\t" //constraint load of remaining variables loaded this way keep pointer to m_accumulate in order to store back in later
+		//PfxVector3 dVA = deltaLinearVelocityA + cross(deltaAngularVelocityA,rA);
+		//PfxVector3 dVB = deltaLinearVelocityB + cross(deltaAngularVelocityB,rB);
+		"vld1.32 	{d14[1]}, [%5]		\n\t" //LOAD => rA for cross product use 1A ---------> q7 (save these for use later)
+		"vld1.32 	{d18[1]}, [%10]		\n\t" //LOAD => rB for cross product use 1B ---------> q9
+		"vld1.32 	{d17[0]}, [%5]!		\n\t" //LOAD => rA for cross product use 2A ---------> q8
+		"vld1.32 	{d21[0]}, [%10]!	\n\t" //LOAD => rB for cross product use 2B ---------> q10
+		"vld1.32 	{d16}, [%5]			\n\t" //rA 2
+		"vld1.32 	{d20}, [%10]		\n\t" //rB 2
+		"vld1.32 	{d15[0]}, [%5]!		\n\t" //rA 1
+		"vld1.32 	{d19[0]}, [%10]!	\n\t" //rB 1
+		"vld1.32 	{d14[0]}, [%5]!		\n\t" //rA 1
+		"vld1.32 	{d18[0]}, [%10]!	\n\t" //rB 1
+		"vld1.32 	{d15[1]}, [%5]		\n\t" //rA 1
+		"vld1.32 	{d19[1]}, [%10]		\n\t" //rB 1
+		"vld1.32 	{d17[1]}, [%5]!		\n\t" //rA 2
+		"vld1.32 	{d21[1]}, [%10]!	\n\t" //rB 2
+		"vmov.f32 	q14, q1				\n\t"
+		"vmov.f32 	q13, q3				\n\t"
+		"vmov.f32 	q15, q1				\n\t"
+		"vmov.f32 	q12, q3				\n\t"
+		"vrev64.32	d28, d28			\n\t" //set deltaAngularVelocityA 1
+		"vtrn.32	d28, d29			\n\t" //set deltaAngularVelocityA 1
+		"vrev64.32	d26, d26			\n\t" //set deltaAngularVelocityB 1
+		"vmul.f32 	q14, q7, q14		\n\t" //operation for cross product 1A
+		"vtrn.32	d26, d27			\n\t" //set deltaAngularVelocityB 1
+		"vtrn.32	d30, d31			\n\t" //set deltaAngularVelocityA 2
+		"vmul.f32 	q13, q9, q13		\n\t" //operation for cross product 1B
+		"vtrn.32	d24, d25			\n\t" //set deltaAngularVelocityB 2
+		"vrev64.32	d30, d30			\n\t" //set deltaAngularVelocityA 2
+		"vrev64.32	d24, d24			\n\t" //set deltaAngularVelocityB 2
+		"vmls.f32 	q14, q8, q15		\n\t" //operation for cross product 2A
+		"vmls.f32 	q13, q10, q12		\n\t" //operation for cross product 2B
+		"vadd.f32 	q14, q14, q0		\n\t" //operation for adding cross to linearVelocityA
+		"vadd.f32 	q13, q13, q2		\n\t" //operation for adding cross to linearVelocityB
+												//LOAD => dVA --------------------------------> q14
+												//LOAD => dVB --------------------------------> q13
+												//FREE q11, q12, q15, d11
+		//deltaImpulse -= constraint.m_jacDiagInv * dot(normal,dVA-dVB);
+		"vsub.f32 	q11, q14, q13		\n\t" //TEMP q11 => dVA-dVB for dot product
+		"vmul.f32 	q11, q11, q4		\n\t" //operation for dot product
+		"vpadd.f32 	d22, d22, d22		\n\t" //operation for dot product
+		"vpadd.f32 	d23, d23, d23		\n\t" //operation for dot product
+		"vadd.f32 	d22, d22, d23		\n\t" //operation for dot product
+		"vmul.f32 	d22, d22, d12[0]	\n\t" //m_jacDiagInv times dot product result
+		"vsub.f32 	d10, d10, d22		\n\t" //subtract result from deltaImpule
+		//PfxFloat oldImpulse = constraint.m_accumImpulse;
+		"vdup.32	d11, d13[1]			\n\t" //LOAD => oldImpulse -------------------------> q5 (d11 only)
+		//constraint.m_accumImpulse = SCE_PFX_CLAMP(oldImpulse + deltaImpulse,constraint.m_lowerLimit,constraint.m_upperLimit);
+		"vdup.32	d23, d13[0]			\n\t" //TEMP q11 => m_upperLimit (d23 only)
+		"vdup.32	d24, d12[1]			\n\t" //TEMP q12 => m_lowerLimit (d34 only)
+		"vadd.f32   d25, d10, d11		\n\t" //TEMP q12 => deltaImpulse + oldImplues (d25 only)
+		"vmin.f32   d25, d25, d23		\n\t" //operation MIN(v,b)
+		"vmax.f32	d22, d25, d24		\n\t" //operation MAX(a,MIN(v,b)
+		"vst1.32    {d22[0]}, [%0]		\n\t" //store m_accumImpulse (incremented so that it can be reloaded for cross product later)
+		//deltaImpulse = constraint.m_accumImpulse - oldImpulse;
+		"vsub.f32 	d10, d22, d11		\n\t" //operation to calculate new deltaImpule
+												//FREE q6, q11, q12, q13, q14, q15, d11
+		//deltaLinearVelocityB -= deltaImpulse * massInvB * normal;
+		//deltaLinearVelocityA += deltaImpulse * massInvA * normal;
+		"vld1.32 	{d11[0]}, [%3]		\n\t" //LOAD => massInvA ---------------------------> q5 (d11[0] only)
+		"vld1.32 	{d11[1]}, [%8]		\n\t" //LOAD => massInvB ---------------------------> q5 (d11[1] only)
+		"vmul.f32	q11, q4, d11[0]		\n\t" //TEMP q11 => operation normal times massInvA A
+		"vmul.f32	q12, q4, d11[1]		\n\t" //TEMP q12 => operation normal times massInvB B
+		"vmul.f32	q11, q11, d10[0]	\n\t" //TEMP q11 => operation result times DeltaImpulse A
+		"vmul.f32	q12, q12, d10[0]	\n\t" //TEMP q12 => operation result times DeltaImpulse B
+		"vadd.f32	q0, q0, q11			\n\t" //operation create new deltaLinearVelocityA A
+		"vst1.32	{q0}, [%1]			\n\t" //store the new deltaLinearVelocityA A
+		"vsub.f32	q2, q2, q12			\n\t" //operation create new deltaLinearVelocityB B
+		"vst1.32	{q2}, [%6]			\n\t" //store the new deltaLinearVelocityB B
+												//FREE q0, q2, q6, q11, q12, q13, q14, q15, d11
+		//deltaAngularVelocityA += deltaImpulse * inertiaInvA * cross(rA,normal);
+		//deltaAngularVelocityB -= deltaImpulse * inertiaInvB * cross(rB,normal);
+		"vmov.f32 	q14, q4				\n\t" //
+		"vmov.f32 	q15, q4				\n\t" //
+		"vtrn.32	d30, d31			\n\t" //
+		"vrev64.32	d30, d30			\n\t" //
+		"vmul.f32 	q0, q8, q15			\n\t" //operation for cross product A
+		"vrev64.32	d28, d28			\n\t" //
+		"vmul.f32 	q2, q10, q15		\n\t" //operation for cross product B
+		"vtrn.32	d28, d29			\n\t" //
+		"vmls.f32 	q0, q14, q7			\n\t" //operation for cross product A
+		"vmls.f32 	q2, q14, q9			\n\t" //operation for cross product B
+										  	  	//LOAD => cross product result A ------------> q0
+												//LOAD => cross product result B ------------> q2
+												//FREE q6, q7, q8, q9, q10, q11, q12, q13, q14, q15, d11
+
+		"vld1.32    {q13-q14}, [%4]! 	\n\t" //LOAD => inertiaInvA col0, col1 A ----------> q13, q14
+		"vld1.32    {q9-q10}, [%9]!	 	\n\t" //LOAD => inertiaInvB col0, col1 B -----------> q9, q10
+		"vld1.32    {q15}, [%4]   	    \n\t" //LOAD => inertiaInvA col2 A ----------------> q5
+		"vld1.32    {q11}, [%9]   	    \n\t" //LOAD => inertiaInvB col2 B -----------------> q11
+		"vmul.f32   q13, q13, d0[0]    	\n\t" //operation inertiaInvA col0 = (col0) * (crossA elem0) A
+		"vmul.f32   q9, q9, d4[0]      	\n\t" //operation inertiaInvB col0 = (col0) * (crossB elem0) B
+		"vmla.f32   q13, q14, d0[1]    	\n\t" //operation inertiaInvA col1 = (col1) * (crossA elem1) A
+		"vmla.f32   q9, q10, d4[1]     	\n\t" //operation inertiaInvB col1 = (col1) * (crossB elem1) B
+		"vmla.f32   q13, q15, d1[0]    	\n\t" //operation inertiaInvA col2 = (col2) * (crossA elem2) A
+		"vmla.f32   q9, q11, d5[0]     	\n\t" //operation inertiaInvB col2 = (col2) * (crossB elem2) B
+		"vmul.f32   q13, q13, d10[0]    \n\t" //operation inertiaInvA times deltaImpulse A
+		"vmul.f32   q9, q9, d10[0]      \n\t" //operation inertiaInvB times deltaImpulse B
+		"vadd.f32   q1, q1, q13      	\n\t" //operation accumulate the deltaAngularVelocityA A
+		"vst1.32    {q1}, [%2]      	\n\t" //store deltaAngularVelocityA A
+		"vsub.f32   q3, q3, q9      	\n\t" //operation accumulate the deltaAngularVelocityB B
+		"vst1.32    {q3}, [%7]      	\n\t" //store deltaAngularVelocityB B
+		: // NO direct outputs! It is important to *not* put anything here. Real output is written directly to memory pointed to by inputs
+		: "r" (&constraint), "r" (&deltaLinearVelocityA), "r" (&deltaAngularVelocityA), "r" (&massInvA), "r" (&inertiaInvA), "r" (&rA), "r" (&deltaLinearVelocityB), "r" (&deltaAngularVelocityB), "r" (&massInvB), "r" (&inertiaInvB), "r" (&rB) //inputs
+		: "memory", "q0", "q1", "q2", "q3", "q4", "q5", "q6", "q7", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15" // clobbers
+	);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif //__ANDROID__

Extras/PhysicsEffects/src/base_level/solver/pfx_contact_constraint.cpp

@@ -0,0 +1,194 @@
+/*
+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/base_level/solver/pfx_contact_constraint.h"
+#include "pfx_constraint_row_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_CONTACT_SLOP 0.001f
+
+void pfxSetupContactConstraint(
+	PfxConstraintRow &constraintResponse,
+	PfxConstraintRow &constraintFriction1,
+	PfxConstraintRow &constraintFriction2,
+	PfxFloat penetrationDepth,
+	PfxFloat restitution,
+	PfxFloat friction,
+	const PfxVector3 &contactNormal,
+	const PfxVector3 &contactPointA,
+	const PfxVector3 &contactPointB,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat separateBias,
+	PfxFloat timeStep
+	)
+{
+	(void)friction;
+
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,contactPointA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,contactPointB);
+
+	PfxFloat massInvA = solverBodyA.m_massInv;
+	PfxFloat massInvB = solverBodyB.m_massInv;
+	PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
+	PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
+
+	if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {
+		massInvB = 0.0f;
+		inertiaInvB = PfxMatrix3(0.0f);
+	}
+	if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {
+		massInvA = 0.0f;
+		inertiaInvA = PfxMatrix3(0.0f);
+	}
+
+	PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(massInvA + massInvB)) - 
+			crossMatrix(rA) * inertiaInvA * crossMatrix(rA) - 
+			crossMatrix(rB) * inertiaInvB * crossMatrix(rB);
+
+	PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
+	PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
+	PfxVector3 vAB = vA-vB;
+
+	PfxVector3 tangent1,tangent2;
+	pfxGetPlaneSpace(contactNormal,tangent1,tangent2);
+
+	// Contact Constraint
+	{
+		PfxVector3 normal = contactNormal;
+
+		PfxFloat denom = dot(K*normal,normal);
+
+		constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error
+		constraintResponse.m_rhs -= (separateBias * SCE_PFX_MIN(0.0f,penetrationDepth+SCE_PFX_CONTACT_SLOP)) / timeStep; // position error
+		constraintResponse.m_rhs /= denom;
+		constraintResponse.m_jacDiagInv = 1.0f/denom;
+		constraintResponse.m_lowerLimit = 0.0f;
+		constraintResponse.m_upperLimit = SCE_PFX_FLT_MAX;
+		pfxStoreVector3(normal,constraintResponse.m_normal);
+	}
+
+	// Friction Constraint 1
+	{
+		PfxVector3 normal = tangent1;
+
+		PfxFloat denom = dot(K*normal,normal);
+
+		constraintFriction1.m_jacDiagInv = 1.0f/denom;
+		constraintFriction1.m_rhs = -dot(vAB,normal);
+		constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;
+		constraintFriction1.m_lowerLimit = 0.0f;
+		constraintFriction1.m_upperLimit = SCE_PFX_FLT_MAX;
+		pfxStoreVector3(normal,constraintFriction1.m_normal);
+	}
+	
+	// Friction Constraint 2
+	{
+		PfxVector3 normal = tangent2;
+
+		PfxFloat denom = dot(K*normal,normal);
+
+		constraintFriction2.m_jacDiagInv = 1.0f/denom;
+		constraintFriction2.m_rhs = -dot(vAB,normal);
+		constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;
+		constraintFriction2.m_lowerLimit = 0.0f;
+		constraintFriction2.m_upperLimit = SCE_PFX_FLT_MAX;
+		pfxStoreVector3(normal,constraintFriction2.m_normal);
+	}
+}
+
+void pfxSolveContactConstraint(
+	PfxConstraintRow &constraintResponse,
+	PfxConstraintRow &constraintFriction1,
+	PfxConstraintRow &constraintFriction2,
+	const PfxVector3 &contactPointA,
+	const PfxVector3 &contactPointB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat friction
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,contactPointA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,contactPointB);
+
+	PfxFloat massInvA = solverBodyA.m_massInv;
+	PfxFloat massInvB = solverBodyB.m_massInv;
+	PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
+	PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
+
+	if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {
+		massInvB = 0.0f;
+		inertiaInvB = PfxMatrix3(0.0f);
+	}
+	if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {
+		massInvA = 0.0f;
+		inertiaInvA = PfxMatrix3(0.0f);
+	}
+
+// ARA begin insert new code
+#ifdef __ARM_NEON__
+	pfxSolveLinearConstraintRowNEON(constraintResponse,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
+#else // __ARM_NEON__
+// ARA end
+
+	pfxSolveLinearConstraintRow(constraintResponse,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
+
+// ARA begin insert new code
+#endif // __ARM_NEON__
+// ARA end
+
+	PfxFloat mf = friction*fabsf(constraintResponse.m_accumImpulse);
+	constraintFriction1.m_lowerLimit = -mf;
+	constraintFriction1.m_upperLimit =  mf;
+	constraintFriction2.m_lowerLimit = -mf;
+	constraintFriction2.m_upperLimit =  mf;
+
+// ARA begin insert new code
+#ifdef __ARM_NEON__
+	pfxSolveLinearConstraintRowNEON(constraintFriction1,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
+
+	pfxSolveLinearConstraintRowNEON(constraintFriction2,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
+#else // __ARM_NEON__
+// ARA end
+
+	pfxSolveLinearConstraintRow(constraintFriction1,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
+
+	pfxSolveLinearConstraintRow(constraintFriction2,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
+
+// ARA begin insert new code
+#endif // __ARM_NEON__
+// ARA end
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/solver/pfx_joint_ball.cpp

@@ -0,0 +1,179 @@
+/*
+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/base_level/solver/pfx_joint_ball.h"
+#include "pfx_constraint_row_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxInitializeBallJoint(PfxJoint &joint,
+	const PfxRigidState &stateA,const PfxRigidState &stateB,
+	const PfxBallJointInitParam &param)
+{
+	joint.m_active = 1;
+	joint.m_numConstraints = 3;
+	joint.m_userData = NULL;
+	joint.m_type = kPfxJointBall;
+	joint.m_rigidBodyIdA = stateA.getRigidBodyId();
+	joint.m_rigidBodyIdB = stateB.getRigidBodyId();
+	
+	for(int i=0;i<3;i++) {
+		joint.m_constraints[i].reset();
+	}
+	
+	joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	
+	PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation()));
+	PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation()));
+	
+	joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition());
+	joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition());
+	
+	joint.m_frameA = PfxMatrix3::identity();
+	joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA;
+	
+	return SCE_PFX_OK;
+}
+
+void pfxSetupBallJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
+
+	PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
+	PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
+	PfxVector3 vAB = vA-vB;
+
+	PfxVector3 distance = (stateA.getPosition() + rA) - (stateB.getPosition() + rB);
+
+	PfxMatrix3 worldFrameA,worldFrameB;
+	worldFrameA = PfxMatrix3(solverBodyA.m_orientation) * joint.m_frameA;
+	worldFrameB = PfxMatrix3(solverBodyB.m_orientation) * joint.m_frameB;
+	
+	// Linear Constraint
+	PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(solverBodyA.m_massInv + solverBodyB.m_massInv)) - 
+			crossMatrix(rA) * solverBodyA.m_inertiaInv * crossMatrix(rA) - 
+			crossMatrix(rB) * solverBodyB.m_inertiaInv * crossMatrix(rB);
+	
+	for(int c=0;c<3;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		PfxVector3 normal = worldFrameA[c];
+		
+		PfxFloat posErr = dot(distance,-normal);
+		PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
+		PfxFloat upperLimit =  jointConstraint.m_maxImpulse;
+		PfxFloat velocityAmp = 1.0f;
+		
+		pfxCalcLinearLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);
+
+		PfxFloat denom = dot(K*normal,normal);
+		
+		constraint.m_rhs = -velocityAmp*dot(vAB,normal);
+		constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep;
+		constraint.m_rhs *= jointConstraint.m_weight/denom;
+		constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom;
+		constraint.m_lowerLimit = lowerLimit;
+		constraint.m_upperLimit = upperLimit;
+		pfxStoreVector3(normal,constraint.m_normal);
+	}
+}
+
+void pfxWarmStartBallJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
+
+	// Linear Constraint
+	for(int c=0;c<3;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		if(jointConstraint.m_warmStarting == 0) {
+			constraint.m_accumImpulse = 0.0f;
+		}
+		else {
+			PfxVector3 normal = pfxReadVector3(constraint.m_normal);
+			PfxFloat deltaImpulse = constraint.m_accumImpulse;
+			solverBodyA.m_deltaLinearVelocity += deltaImpulse * solverBodyA.m_massInv * normal;
+			solverBodyA.m_deltaAngularVelocity += deltaImpulse * solverBodyA.m_inertiaInv * cross(rA,normal);
+			solverBodyB.m_deltaLinearVelocity -= deltaImpulse * solverBodyB.m_massInv * normal;
+			solverBodyB.m_deltaAngularVelocity -= deltaImpulse * solverBodyB.m_inertiaInv * cross(rB,normal);
+		}
+	}
+}
+
+void pfxSolveBallJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
+
+	// Linear Constraint
+// ARA begin insert new code
+#ifdef __ARM_NEON__
+	pfxSolveLinearConstraintRowNEON(joint.m_constraints[0].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRowNEON(joint.m_constraints[1].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRowNEON(joint.m_constraints[2].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+#else // __ARM_NEON__
+// ARA end
+
+	pfxSolveLinearConstraintRow(joint.m_constraints[0].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRow(joint.m_constraints[1].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRow(joint.m_constraints[2].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+// ARA begin insert new code
+#endif // __ARM_NEON__
+// ARA end
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/solver/pfx_joint_fix.cpp

@@ -0,0 +1,54 @@
+/*
+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/base_level/solver/pfx_joint_fix.h"
+#include "pfx_constraint_row_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxInitializeFixJoint(PfxJoint &joint,
+	const PfxRigidState &stateA,const PfxRigidState &stateB,
+	const PfxFixJointInitParam &param)
+{
+	joint.m_active = 1;
+	joint.m_numConstraints = 6;
+	joint.m_userData = NULL;
+	joint.m_type = kPfxJointFix;
+	joint.m_rigidBodyIdA = stateA.getRigidBodyId();
+	joint.m_rigidBodyIdB = stateB.getRigidBodyId();
+	
+	for(int i=0;i<6;i++) {
+		joint.m_constraints[i].reset();
+		joint.m_constraints[i].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	}
+	
+	// Calc joint frame
+	PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation()));
+	PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation()));
+	
+	joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition());
+	joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition());
+	
+	joint.m_frameA = PfxMatrix3::identity();
+	joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA;
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/solver/pfx_joint_hinge.cpp

@@ -0,0 +1,80 @@
+/*
+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/base_level/solver/pfx_joint_hinge.h"
+#include "pfx_constraint_row_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxInitializeHingeJoint(PfxJoint &joint,
+	const PfxRigidState &stateA,const PfxRigidState &stateB,
+	const PfxHingeJointInitParam &param)
+{
+	joint.m_active = 1;
+	joint.m_numConstraints = 6;
+	joint.m_userData = NULL;
+	joint.m_type = kPfxJointHinge;
+	joint.m_rigidBodyIdA = stateA.getRigidBodyId();
+	joint.m_rigidBodyIdB = stateB.getRigidBodyId();
+	
+	for(int i=0;i<6;i++) {
+		joint.m_constraints[i].reset();
+	}
+	
+	joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	if(param.lowerAngle == 0.0f && param.upperAngle == 0.0f) {
+		joint.m_constraints[3].m_lock = SCE_PFX_JOINT_LOCK_FREE;
+	}
+	else {
+		joint.m_constraints[3].m_lock = SCE_PFX_JOINT_LOCK_LIMIT;
+	}
+	joint.m_constraints[4].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[5].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	
+	// Set twist angle limit
+	if(param.lowerAngle > param.upperAngle || 
+		!SCE_PFX_RANGE_CHECK(param.lowerAngle,-SCE_PFX_PI,SCE_PFX_PI) || 
+		!SCE_PFX_RANGE_CHECK(param.upperAngle,-SCE_PFX_PI,SCE_PFX_PI)) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	joint.m_constraints[3].m_movableLowerLimit = param.lowerAngle;
+	joint.m_constraints[3].m_movableUpperLimit = param.upperAngle;
+	
+	// Calc joint frame
+	PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation()));
+	PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation()));
+	
+	PfxVector3 axisInA = rotA * normalize(param.axis);
+	
+	joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition());
+	joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition());
+	
+	PfxVector3 axis1, axis2;
+	
+	pfxGetPlaneSpace(axisInA, axis1, axis2 );
+	
+	joint.m_frameA = PfxMatrix3(axisInA, axis1, axis2);
+	joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA;
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/solver/pfx_joint_slider.cpp

@@ -0,0 +1,78 @@
+/*
+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/base_level/solver/pfx_joint_slider.h"
+#include "pfx_constraint_row_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxInitializeSliderJoint(PfxJoint &joint,
+	const PfxRigidState &stateA,const PfxRigidState &stateB,
+	const PfxSliderJointInitParam &param)
+{
+	joint.m_active = 1;
+	joint.m_numConstraints = 6;
+	joint.m_userData = NULL;
+	joint.m_type = kPfxJointSlider;
+	joint.m_rigidBodyIdA = stateA.getRigidBodyId();
+	joint.m_rigidBodyIdB = stateB.getRigidBodyId();
+	
+	for(int i=0;i<6;i++) {
+		joint.m_constraints[i].reset();
+	}
+
+	if(param.lowerDistance == 0.0f && param.upperDistance == 0.0f) {
+		joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FREE;
+	}
+	else {
+		joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_LIMIT;
+	}
+	joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[3].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[4].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[5].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	
+	if(param.lowerDistance > param.upperDistance ) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	
+	joint.m_constraints[0].m_movableLowerLimit = param.lowerDistance;
+	joint.m_constraints[0].m_movableUpperLimit = param.upperDistance;
+	
+	// Calc joint frame
+	PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation()));
+	PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation()));
+	
+	PfxVector3 axisInA = rotA * normalize(param.direction);
+	
+	joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition());
+	joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition());
+	
+	PfxVector3 axis1, axis2;
+	
+	pfxGetPlaneSpace(axisInA, axis1, axis2 );
+	
+	joint.m_frameA = PfxMatrix3(axisInA, axis1, axis2);
+	joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA;
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/solver/pfx_joint_swing_twist.cpp

@@ -0,0 +1,268 @@
+/*
+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/base_level/solver/pfx_joint_swing_twist.h"
+#include "pfx_constraint_row_solver.h"
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxInitializeSwingTwistJoint(PfxJoint &joint,
+	const PfxRigidState &stateA,const PfxRigidState &stateB,
+	const PfxSwingTwistJointInitParam &param)
+{
+	joint.m_active = 1;
+	joint.m_numConstraints = 6;
+	joint.m_userData = NULL;
+	joint.m_type = kPfxJointSwingtwist;
+	joint.m_rigidBodyIdA = stateA.getRigidBodyId();
+	joint.m_rigidBodyIdB = stateB.getRigidBodyId();
+	
+	for(int i=0;i<6;i++) {
+		joint.m_constraints[i].reset();
+	}
+	
+	joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[3].m_lock = SCE_PFX_JOINT_LOCK_LIMIT;
+	joint.m_constraints[4].m_lock = SCE_PFX_JOINT_LOCK_LIMIT;
+	joint.m_constraints[5].m_lock = SCE_PFX_JOINT_LOCK_FREE;
+	
+	// Set twist angle limit
+	if(param.twistLowerAngle > param.twistUpperAngle || 
+		!SCE_PFX_RANGE_CHECK(param.twistLowerAngle,-SCE_PFX_PI,SCE_PFX_PI) || 
+		!SCE_PFX_RANGE_CHECK(param.twistUpperAngle,-SCE_PFX_PI,SCE_PFX_PI)) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	joint.m_constraints[3].m_movableLowerLimit = param.twistLowerAngle;
+	joint.m_constraints[3].m_movableUpperLimit = param.twistUpperAngle;
+	
+	// Set swing angle limit
+	if(param.swingLowerAngle > param.swingUpperAngle || 
+		!SCE_PFX_RANGE_CHECK(param.swingLowerAngle,0.0f,SCE_PFX_PI) || 
+		!SCE_PFX_RANGE_CHECK(param.swingUpperAngle,0.0f,SCE_PFX_PI)) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	joint.m_constraints[4].m_movableLowerLimit = param.swingLowerAngle;
+	joint.m_constraints[4].m_movableUpperLimit = param.swingUpperAngle;
+	
+	// Calc joint frame
+	PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation()));
+	PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation()));
+	
+	PfxVector3 axisInA = rotA * normalize(param.twistAxis);
+	
+	joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition());
+	joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition());
+	
+	PfxVector3 axis1, axis2;
+	
+	pfxGetPlaneSpace(axisInA, axis1, axis2 );
+	
+	joint.m_frameA = PfxMatrix3(axisInA, axis1, axis2);
+	joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA;
+	
+	return SCE_PFX_OK;
+}
+
+void pfxSetupSwingTwistJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
+
+	PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
+	PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
+	PfxVector3 vAB = vA-vB;
+
+	PfxVector3 angAB = stateA.getAngularVelocity() - stateB.getAngularVelocity();
+
+	PfxVector3 distance = (stateA.getPosition() + rA) - (stateB.getPosition() + rB);
+
+	PfxMatrix3 worldFrameA,worldFrameB;
+	worldFrameA = PfxMatrix3(solverBodyA.m_orientation) * joint.m_frameA;
+	worldFrameB = PfxMatrix3(solverBodyB.m_orientation) * joint.m_frameB;
+	
+	// Linear Constraint
+	PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(solverBodyA.m_massInv + solverBodyB.m_massInv)) - 
+			crossMatrix(rA) * solverBodyA.m_inertiaInv * crossMatrix(rA) - 
+			crossMatrix(rB) * solverBodyB.m_inertiaInv * crossMatrix(rB);
+	
+	for(int c=0;c<3;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		PfxVector3 normal = worldFrameA[c];
+		
+		PfxFloat posErr = dot(distance,-normal);
+		PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
+		PfxFloat upperLimit =  jointConstraint.m_maxImpulse;
+		PfxFloat velocityAmp = 1.0f;
+		
+		pfxCalcLinearLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);
+
+		PfxFloat denom = dot(K*normal,normal);
+		
+		constraint.m_rhs = -velocityAmp*dot(vAB,normal);
+		constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep;
+		constraint.m_rhs *= jointConstraint.m_weight/denom;
+		constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom;
+		constraint.m_lowerLimit = lowerLimit;
+		constraint.m_upperLimit = upperLimit;
+		pfxStoreVector3(normal,constraint.m_normal);
+	}
+
+	PfxVector3 axis[3];
+	PfxFloat angle[3];
+	pfxCalcJointAngleSwingTwist(worldFrameA,worldFrameB,angle,axis);
+
+	// Angular Constraint
+	for(int c=3;c<6;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		PfxVector3 normal = axis[c-3];
+
+		PfxFloat posErr = angle[c-3];
+		PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
+		PfxFloat upperLimit =  jointConstraint.m_maxImpulse;
+		PfxFloat velocityAmp = 1.0f;
+		
+		pfxCalcAngularLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);
+
+		PfxFloat denom = dot((solverBodyA.m_inertiaInv+solverBodyB.m_inertiaInv)*normal,normal);
+		
+		constraint.m_rhs = -velocityAmp*dot(angAB,normal); // velocity error
+		constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep; // position error
+		constraint.m_rhs *= jointConstraint.m_weight/denom;
+		constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom;
+		constraint.m_lowerLimit = lowerLimit;
+		constraint.m_upperLimit = upperLimit;
+		pfxStoreVector3(normal,constraint.m_normal);
+	}
+}
+
+void pfxWarmStartSwingTwistJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
+
+	// Linear Constraint
+	for(int c=0;c<3;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		if(jointConstraint.m_warmStarting == 0) {
+			constraint.m_accumImpulse = 0.0f;
+		}
+		else {
+			PfxVector3 normal = pfxReadVector3(constraint.m_normal);
+			PfxFloat deltaImpulse = constraint.m_accumImpulse;
+			solverBodyA.m_deltaLinearVelocity += deltaImpulse * solverBodyA.m_massInv * normal;
+			solverBodyA.m_deltaAngularVelocity += deltaImpulse * solverBodyA.m_inertiaInv * cross(rA,normal);
+			solverBodyB.m_deltaLinearVelocity -= deltaImpulse * solverBodyB.m_massInv * normal;
+			solverBodyB.m_deltaAngularVelocity -= deltaImpulse * solverBodyB.m_inertiaInv * cross(rB,normal);
+		}
+	}
+
+	// Angular Constraint
+	for(int c=3;c<6;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		if(jointConstraint.m_warmStarting == 0) {
+			constraint.m_accumImpulse = 0.0f;
+		}
+		else {
+			PfxVector3 normal = pfxReadVector3(constraint.m_normal);
+			PfxFloat deltaImpulse = constraint.m_accumImpulse;
+			solverBodyA.m_deltaAngularVelocity += deltaImpulse * solverBodyA.m_inertiaInv * normal;
+			solverBodyB.m_deltaAngularVelocity -= deltaImpulse * solverBodyB.m_inertiaInv * normal;
+		}
+	}
+}
+
+void pfxSolveSwingTwistJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
+
+	// Linear Constraint
+// ARA begin insert new code
+#ifdef __ARM_NEON__
+	pfxSolveLinearConstraintRowNEON(joint.m_constraints[0].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRowNEON(joint.m_constraints[1].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRowNEON(joint.m_constraints[2].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+#else // __ARM_NEON__
+// ARA end
+
+	pfxSolveLinearConstraintRow(joint.m_constraints[0].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRow(joint.m_constraints[1].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveLinearConstraintRow(joint.m_constraints[2].m_constraintRow,
+		solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
+
+// ARA begin insert new code
+#endif // __ARM_NEON__
+// ARA end
+
+	// Angular Constraint
+	pfxSolveAngularConstraintRow(joint.m_constraints[3].m_constraintRow,
+		solverBodyA.m_deltaAngularVelocity,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaAngularVelocity,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveAngularConstraintRow(joint.m_constraints[4].m_constraintRow,
+		solverBodyA.m_deltaAngularVelocity,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaAngularVelocity,solverBodyB.m_inertiaInv,rB);
+
+	pfxSolveAngularConstraintRow(joint.m_constraints[5].m_constraintRow,
+		solverBodyA.m_deltaAngularVelocity,solverBodyA.m_inertiaInv,rA,
+		solverBodyB.m_deltaAngularVelocity,solverBodyB.m_inertiaInv,rB);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/solver/pfx_joint_universal.cpp

@@ -0,0 +1,163 @@
+/*
+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/base_level/solver/pfx_joint_universal.h"
+#include "pfx_constraint_row_solver.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxInitializeUniversalJoint(PfxJoint &joint,
+	const PfxRigidState &stateA,const PfxRigidState &stateB,
+	const PfxUniversalJointInitParam &param)
+{
+	joint.m_active = 1;
+	joint.m_numConstraints = 6;
+	joint.m_userData = NULL;
+	joint.m_type = kPfxJointUniversal;
+	joint.m_rigidBodyIdA = stateA.getRigidBodyId();
+	joint.m_rigidBodyIdB = stateB.getRigidBodyId();
+	
+	for(int i=0;i<6;i++) {
+		joint.m_constraints[i].reset();
+	}
+	
+	joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[3].m_lock = SCE_PFX_JOINT_LOCK_FIX;
+	joint.m_constraints[4].m_lock = SCE_PFX_JOINT_LOCK_LIMIT;
+	joint.m_constraints[5].m_lock = SCE_PFX_JOINT_LOCK_LIMIT;
+	
+	// Set swing angle limit
+	if(param.swing1LowerAngle > param.swing1UpperAngle || 
+		!SCE_PFX_RANGE_CHECK(param.swing1LowerAngle,-SCE_PFX_PI,SCE_PFX_PI) || 
+		!SCE_PFX_RANGE_CHECK(param.swing1UpperAngle,-SCE_PFX_PI,SCE_PFX_PI) || 
+		param.swing2LowerAngle > param.swing2UpperAngle || 
+		!SCE_PFX_RANGE_CHECK(param.swing2LowerAngle,-SCE_PFX_PI,SCE_PFX_PI) || 
+		!SCE_PFX_RANGE_CHECK(param.swing2UpperAngle,-SCE_PFX_PI,SCE_PFX_PI)) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	
+	joint.m_constraints[4].m_movableLowerLimit = param.swing1LowerAngle;
+	joint.m_constraints[4].m_movableUpperLimit = param.swing1UpperAngle;
+	joint.m_constraints[5].m_movableLowerLimit = param.swing2LowerAngle;
+	joint.m_constraints[5].m_movableUpperLimit = param.swing2UpperAngle;
+	
+	// Calc joint frame
+	PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation()));
+	PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation()));
+	
+	PfxVector3 axisInA = rotA * normalize(param.axis);
+	
+	joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition());
+	joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition());
+	
+	PfxVector3 axis1, axis2;
+	
+	pfxGetPlaneSpace(axisInA, axis1, axis2 );
+	
+	joint.m_frameA = PfxMatrix3(axisInA, axis1, axis2);
+	joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA;
+	
+	return SCE_PFX_OK;
+}
+
+void pfxSetupUniversalJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep
+	)
+{
+	PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
+	PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
+
+	PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
+	PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
+	PfxVector3 vAB = vA-vB;
+
+	PfxVector3 angAB = stateA.getAngularVelocity() - stateB.getAngularVelocity();
+
+	PfxVector3 distance = (stateA.getPosition() + rA) - (stateB.getPosition() + rB);
+
+	PfxMatrix3 worldFrameA,worldFrameB;
+	worldFrameA = PfxMatrix3(solverBodyA.m_orientation) * joint.m_frameA;
+	worldFrameB = PfxMatrix3(solverBodyB.m_orientation) * joint.m_frameB;
+	
+	// Linear Constraint
+	PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(solverBodyA.m_massInv + solverBodyB.m_massInv)) - 
+			crossMatrix(rA) * solverBodyA.m_inertiaInv * crossMatrix(rA) - 
+			crossMatrix(rB) * solverBodyB.m_inertiaInv * crossMatrix(rB);
+	
+	for(int c=0;c<3;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		PfxVector3 normal = worldFrameA[c];
+		
+		PfxFloat posErr = dot(distance,-normal);
+		PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
+		PfxFloat upperLimit =  jointConstraint.m_maxImpulse;
+		PfxFloat velocityAmp = 1.0f;
+		
+		pfxCalcLinearLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);
+
+		PfxFloat denom = dot(K*normal,normal);
+		
+		constraint.m_rhs = -velocityAmp*dot(vAB,normal);
+		constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep;
+		constraint.m_rhs *= jointConstraint.m_weight/denom;
+		constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom;
+		constraint.m_lowerLimit = lowerLimit;
+		constraint.m_upperLimit = upperLimit;
+		pfxStoreVector3(normal,constraint.m_normal);
+	}
+
+	PfxVector3 axis[3];
+	PfxFloat angle[3];
+	pfxCalcJointAngleUniversal(worldFrameA,worldFrameB,angle,axis);
+
+	// Angular Constraint
+	for(int c=3;c<6;c++) {
+		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
+		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
+
+		PfxVector3 normal = axis[c-3];
+
+		PfxFloat posErr = angle[c-3];
+		PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
+		PfxFloat upperLimit =  jointConstraint.m_maxImpulse;
+		PfxFloat velocityAmp = 1.0f;
+		
+		pfxCalcAngularLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);
+
+		PfxFloat denom = dot((solverBodyA.m_inertiaInv+solverBodyB.m_inertiaInv)*normal,normal);
+		
+		constraint.m_rhs = -velocityAmp*dot(angAB,normal); // velocity error
+		constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep; // position error
+		constraint.m_rhs *= jointConstraint.m_weight/denom;
+		constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom;
+		constraint.m_lowerLimit = lowerLimit;
+		constraint.m_upperLimit = upperLimit;
+		pfxStoreVector3(normal,constraint.m_normal);
+	}
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/base_level/sort/pfx_sort.cpp

@@ -0,0 +1,267 @@
+/*
+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/sort/pfx_sort.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+#define Key(a) pfxGetKey(a)
+
+///////////////////////////////////////////////////////////////////////////////
+// Merge Sort
+
+
+
+template <class SortData>
+SCE_PFX_FORCE_INLINE
+void pfxCompareAndSwap(SortData &d1,SortData &d2)
+{
+if(Key(d1) > Key(d2)) {
+	SortData tmp = d1;
+	d1 = d2;
+	d2 = tmp;
+}
+}
+
+template <class SortData>
+SCE_PFX_FORCE_INLINE
+void pfxMergeTwoBuffers11(SortData* d1,int n1,SortData* d2,int n2)
+{
+(void)n1,(void)n2,(void)d2;
+pfxCompareAndSwap(d1[0],d1[1]);
+}
+
+template <class SortData>
+SCE_PFX_FORCE_INLINE
+void pfxMergeTwoBuffers12(SortData* d1,int n1,SortData* d2,int n2)
+{
+(void)n1,(void)d2,(void)n2;
+pfxCompareAndSwap(d1[0],d1[1]);
+pfxCompareAndSwap(d1[1],d1[2]);
+}
+
+template <class SortData>
+SCE_PFX_FORCE_INLINE
+void pfxMergeTwoBuffers22(SortData* d1,int n1,SortData* d2,int n2)
+{
+(void)n1,(void)d2,(void)n2;
+pfxCompareAndSwap(d1[0],d1[2]);
+pfxCompareAndSwap(d1[1],d1[3]);
+pfxCompareAndSwap(d1[1],d1[2]);
+}
+
+template <class SortData>
+SCE_PFX_FORCE_INLINE
+void pfxMergeTwoBuffers23(SortData* d1,int n1,SortData* d2,int n2)
+{
+(void)n1,(void)d2,(void)n2;
+pfxCompareAndSwap(d1[0],d1[2]);
+pfxCompareAndSwap(d1[1],d1[4]);
+pfxCompareAndSwap(d1[2],d1[3]);
+pfxCompareAndSwap(d1[1],d1[2]);
+pfxCompareAndSwap(d1[2],d1[3]);
+}
+
+template <class SortData>
+SCE_PFX_FORCE_INLINE
+void pfxMergeTwoBuffers33(SortData* d1,int n1,SortData* d2,int n2)
+{
+(void)n1,(void)d2,(void)n2;
+pfxCompareAndSwap(d1[0],d1[3]);
+pfxCompareAndSwap(d1[2],d1[5]);
+pfxCompareAndSwap(d1[1],d1[2]);
+pfxCompareAndSwap(d1[3],d1[4]);
+pfxCompareAndSwap(d1[1],d1[3]);
+pfxCompareAndSwap(d1[2],d1[4]);
+pfxCompareAndSwap(d1[2],d1[3]);
+}
+
+template <class SortData>
+void pfxMergeTwoBuffers(SortData* d1,unsigned int n1,SortData* d2,unsigned int n2,SortData *buff)
+{
+unsigned int i=0,j=0;
+
+while(i<n1&&j<n2) {
+	if(Key(d1[i]) < Key(d2[j])) {
+		buff[i+j] = d1[i++];
+	}
+	else {
+		buff[i+j] = d2[j++];
+	}
+}
+
+if(i<n1) {
+	while(i<n1) {
+		buff[i+j] = d1[i++];
+	}
+}
+else if(j<n2) {
+	while(j<n2) {
+		buff[i+j] = d2[j++];
+	}
+}
+
+for(unsigned int k=0;k<(n1+n2);k++) {
+	d1[k] = buff[k];
+}
+}
+
+template <class SortData>
+void pfxMergeSort(SortData *d,SortData *buff,int n)
+{
+int n1 = n>>1;
+int n2 = n-n1;
+if(n1>1) pfxMergeSort(d,buff,n1);
+if(n2>1) pfxMergeSort(d+n1,buff,n2);
+
+int nadd = n1+n2;
+
+if(nadd==2) {
+	pfxMergeTwoBuffers11(d,n1,d+n1,n2);
+}
+else if(nadd==3) {
+	pfxMergeTwoBuffers12(d,n1,d+n1,n2);
+}
+else if(nadd==4) {
+	pfxMergeTwoBuffers22(d,n1,d+n1,n2);
+}
+else if(nadd==5) {
+	pfxMergeTwoBuffers23(d,n1,d+n1,n2);
+}
+else if(nadd==6) {
+	pfxMergeTwoBuffers33(d,n1,d+n1,n2);
+}
+else {
+	pfxMergeTwoBuffers(d,n1,d+n1,n2,buff);
+}
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Bitonic Sort
+
+
+template <class SortData>
+void pfxBitonicMerge(SortData *d,unsigned int n,int dir)
+{
+if(n > 1) {
+	unsigned int k = n>>1;
+	for(unsigned int i=0;i<k;i++) {
+		if(dir==0 && Key(d[i]) > Key(d[i+k])) {
+			SortData t = d[i+k];
+			d[i+k] = d[i];
+			d[i] = t;
+		}
+		if(dir==1 && Key(d[i]) < Key(d[i+k])) {
+			SortData t = d[i+k];
+			d[i+k] = d[i];
+			d[i] = t;
+		}
+	}
+	pfxBitonicMerge(d,k,dir);
+	pfxBitonicMerge(d+k,k,dir);
+}
+}
+
+template <class SortData>
+void pfxBitonicSortInternal(SortData *d,unsigned int n,unsigned int dir=0)
+{
+if(n > 1) {
+	unsigned int k = n>>1;
+	pfxBitonicSortInternal(d,k,0);
+	pfxBitonicSortInternal(d+k,k,1);
+	pfxBitonicMerge(d,n,dir);
+}
+}
+
+template <class SortData>
+void pfxBitonicSort(SortData *d,SortData *buff,unsigned int n)
+{
+pfxBitonicSortInternal(d,n,0);
+memcpy(buff,d,sizeof(SortData)*n);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Hybrid Sort (Bitonic + Merge)
+
+struct PfxDiv2n {
+	unsigned int id;
+	unsigned int num;
+
+	PfxDiv2n() {}
+
+	PfxDiv2n(unsigned int i,unsigned int n)
+	{
+		id = i;
+		num = n;
+	}
+};
+
+template <class SortData>
+void pfxHybridSort(SortData *data,SortData *buff,unsigned int n)
+{
+	unsigned int numDiv = 0;
+	PfxDiv2n divData[32];
+
+	unsigned int id = 0;
+	unsigned int rest = n;
+	unsigned int mask = 0x01;
+
+	while(rest>0) {
+		if((mask&n)>0) {
+			divData[numDiv++] = PfxDiv2n(id,mask);
+			pfxBitonicSort(data+id,buff+id,mask);
+			rest ^= mask;
+			id += mask;
+		}
+		mask <<= 1;
+	}
+
+	if(numDiv==1) {
+		for(unsigned int i=0;i<n;i++) {
+			data[i] = buff[i];
+		}
+	}
+	else {
+		for(unsigned int i=1;i<numDiv;i++) {
+			pfxMergeTwoBuffers(
+				buff+divData[i-1].id,divData[i-1].num,
+				buff+divData[i].id,divData[i].num,
+				data);
+			divData[i].id = divData[i-1].id;
+			divData[i].num += divData[i-1].num;
+		}
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Single Sort
+
+void pfxSort(PfxSortData16 *data,PfxSortData16 *buff,unsigned int n)
+{
+pfxMergeSort(data,buff,n);
+}
+
+void pfxSort(PfxSortData32 *data,PfxSortData32 *buff,unsigned int n)
+{
+pfxMergeSort(data,buff,n);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/CMakeLists.txt

@@ -0,0 +1,29 @@
+INCLUDE_DIRECTORIES( ${PHYSICS_EFFECTS_SOURCE_DIR}/include  )
+
+SET(PfxLowLevel_SRCS
+					broadphase/pfx_broadphase_single.cpp
+					collision/pfx_batched_ray_cast_single.cpp
+					collision/pfx_collision_detection_single.cpp
+					collision/pfx_detect_collision_func.cpp
+					collision/pfx_intersect_ray_func.cpp
+					collision/pfx_island_generation.cpp
+					collision/pfx_ray_cast.cpp
+					collision/pfx_refresh_contacts_single.cpp
+					solver/pfx_constraint_solver_single.cpp
+					solver/pfx_joint_constraint_func.cpp
+					solver/pfx_update_rigid_states_single.cpp
+					sort/pfx_parallel_sort_single.cpp
+)
+
+SET(PfxLowLevel_HDRS
+					collision/pfx_detect_collision_func.h
+					collision/pfx_intersect_ray_func.h
+)
+
+
+
+
+ADD_LIBRARY(PfxLowLevel ${PfxLowLevel_SRCS} ${PfxLowLevel_HDRS})
+
+SET_TARGET_PROPERTIES(PfxLowLevel PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(PfxLowLevel PROPERTIES SOVERSION ${BULLET_VERSION})

Extras/PhysicsEffects/src/low_level/broadphase/pfx_broadphase_single.cpp

@@ -0,0 +1,294 @@
+/*
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
+#include "../../../include/physics_effects/base_level/broadphase/pfx_update_broadphase_proxy.h"
+#include "../../../include/physics_effects/low_level/broadphase/pfx_broadphase.h"
+#include "../../base_level/broadphase/pfx_check_collidable.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxUInt32 pfxGetWorkBytesOfUpdateBroadphaseProxies(PfxUInt32 numRigidBodies)
+{
+	return 128 + SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphaseProxy) * numRigidBodies);
+}
+
+PfxUInt32 pfxGetWorkBytesOfUpdateBroadphaseProxies(PfxUInt32 numRigidBodies,PfxUInt32 maxTasks)
+{
+	(void)maxTasks;
+	return 128 + SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphaseProxy) * numRigidBodies) * 6;
+}
+
+PfxUInt32 pfxGetWorkBytesOfFindPairs(PfxUInt32 maxPairs,PfxUInt32 maxTasks)
+{
+	return 128 + SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair) * maxPairs) * maxTasks;
+}
+
+PfxUInt32 pfxGetPairBytesOfFindPairs(PfxUInt32 maxPairs)
+{
+	return SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair) * maxPairs);
+}
+
+PfxUInt32 pfxGetWorkBytesOfDecomposePairs(PfxUInt32 numPreviousPairs,PfxUInt32 numCurrentPairs,int maxTasks)
+{
+	return 128 +
+		SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair)*numPreviousPairs) + 
+		SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair)*numPreviousPairs) * maxTasks + 
+		SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxBroadphasePair)*numCurrentPairs) * maxTasks;
+}
+
+PfxInt32 pfxCheckParamOfUpdateBroadphaseProxies(const PfxUpdateBroadphaseProxiesParam &param)
+{
+	if(!param.workBuff || 
+		!param.proxiesX || !param.proxiesY || !param.proxiesZ ||
+		!param.proxiesXb || !param.proxiesYb || !param.proxiesZb ||
+		!param.offsetRigidStates || !param.offsetCollidables ) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ) ||
+		!SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb) || !SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb)	) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfUpdateBroadphaseProxies(param.numRigidBodies) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxCheckParamOfFindPairs(const PfxFindPairsParam &param,int maxTasks)
+{
+	if(!param.workBuff || !param.pairBuff || !param.proxies || param.axis > 2 || param.axis < 0) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.proxies) || !SCE_PFX_PTR_IS_ALIGNED16(param.pairBuff)) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfFindPairs(param.maxPairs,maxTasks) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.pairBuff,param.pairBytes) < pfxGetPairBytesOfFindPairs(param.maxPairs) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	return SCE_PFX_OK;
+}
+
+PfxUInt32 pfxGetPairBytesOfDecomposePairs(PfxUInt32 numPreviousPairs,PfxUInt32 numCurrentPairs)
+{
+	return sizeof(PfxBroadphasePair)*(numPreviousPairs*2+numCurrentPairs);
+}
+
+PfxInt32 pfxCheckParamOfDecomposePairs(const PfxDecomposePairsParam &param,int maxTasks)
+{
+	if(!param.workBuff || !param.pairBuff || !param.previousPairs || !param.currentPairs) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.previousPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.currentPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.pairBuff) ) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfDecomposePairs(param.numPreviousPairs,param.numCurrentPairs,maxTasks)) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.pairBuff,param.pairBytes) < pfxGetPairBytesOfDecomposePairs(param.numPreviousPairs,param.numCurrentPairs)) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// SINGLE THREAD
+
+PfxInt32 pfxUpdateBroadphaseProxies(PfxUpdateBroadphaseProxiesParam &param,PfxUpdateBroadphaseProxiesResult &result)
+{
+	PfxInt32 ret = pfxCheckParamOfUpdateBroadphaseProxies(param);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxUpdateBroadphaseProxies")
+
+	result.numOutOfWorldProxies = 0;
+
+	for(int i=0;i<(int)param.numRigidBodies;i++) {
+		PfxInt32 chk = pfxUpdateBroadphaseProxy(
+			param.proxiesX[i],
+			param.proxiesY[i],
+			param.proxiesZ[i],
+			param.proxiesXb[i],
+			param.proxiesYb[i],
+			param.proxiesZb[i],
+			param.offsetRigidStates[i],
+			param.offsetCollidables[i],
+			param.worldCenter,
+			param.worldExtent);
+
+		if(chk == SCE_PFX_ERR_OUT_OF_WORLD) {
+			result.numOutOfWorldProxies++;
+
+			if(param.outOfWorldBehavior & SCE_PFX_OUT_OF_WORLD_BEHAVIOR_FIX_MOTION) {
+				PfxRigidState &state = param.offsetRigidStates[i];
+				state.setMotionType(kPfxMotionTypeFixed);
+				pfxSetMotionMask(param.proxiesX[i],state.getMotionMask());
+				pfxSetMotionMask(param.proxiesY[i],state.getMotionMask());
+				pfxSetMotionMask(param.proxiesZ[i],state.getMotionMask());
+				pfxSetMotionMask(param.proxiesXb[i],state.getMotionMask());
+				pfxSetMotionMask(param.proxiesYb[i],state.getMotionMask());
+				pfxSetMotionMask(param.proxiesZb[i],state.getMotionMask());
+			}
+			
+			if(param.outOfWorldBehavior & SCE_PFX_OUT_OF_WORLD_BEHAVIOR_REMOVE_PROXY) {
+				pfxSetKey(param.proxiesX[i],SCE_PFX_SENTINEL_KEY);
+				pfxSetKey(param.proxiesY[i],SCE_PFX_SENTINEL_KEY);
+				pfxSetKey(param.proxiesZ[i],SCE_PFX_SENTINEL_KEY);
+				pfxSetKey(param.proxiesXb[i],SCE_PFX_SENTINEL_KEY);
+				pfxSetKey(param.proxiesYb[i],SCE_PFX_SENTINEL_KEY);
+				pfxSetKey(param.proxiesZb[i],SCE_PFX_SENTINEL_KEY);
+			}
+		}
+	}
+	
+	PfxHeapManager pool((unsigned char*)param.workBuff,param.workBytes);
+	PfxBroadphaseProxy *workProxies = (PfxBroadphaseProxy*)pool.allocate(sizeof(PfxBroadphaseProxy)*param.numRigidBodies,PfxHeapManager::ALIGN128);
+	
+	pfxSort(param.proxiesX,workProxies,param.numRigidBodies);
+	pfxSort(param.proxiesY,workProxies,param.numRigidBodies);
+	pfxSort(param.proxiesZ,workProxies,param.numRigidBodies);
+	pfxSort(param.proxiesXb,workProxies,param.numRigidBodies);
+	pfxSort(param.proxiesYb,workProxies,param.numRigidBodies);
+	pfxSort(param.proxiesZb,workProxies,param.numRigidBodies);
+	
+	pool.deallocate(workProxies);
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxFindPairs(PfxFindPairsParam &param,PfxFindPairsResult &result)
+{
+	PfxInt32 ret = pfxCheckParamOfFindPairs(param,0);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxFindPairs")
+
+	void *workBuff = param.workBuff;
+	PfxUInt32 workBytes = param.workBytes;
+	PfxBroadphaseProxy *proxies = param.proxies;
+	PfxUInt32 numProxies = param.numProxies;
+	PfxUInt32 maxPairs = param.maxPairs;
+	int axis = param.axis;
+
+	(void) workBytes;
+
+	PfxBroadphasePair *pairs = (PfxBroadphasePair*)SCE_PFX_PTR_ALIGN16(param.pairBuff);
+	PfxUInt32 numPairs = 0;
+	
+	for(PfxUInt32 i=0;i<numProxies;i++) {
+		for(PfxUInt32 j=i+1;j<numProxies;j++) {
+			PfxBroadphaseProxy proxyA,proxyB;
+			if(pfxGetObjectId(proxies[i]) < pfxGetObjectId(proxies[j])) {
+				proxyA = proxies[i];
+				proxyB = proxies[j];
+			}
+			else {
+				proxyA = proxies[j];
+				proxyB = proxies[i];
+			}
+			
+			if(pfxGetXYZMax(proxyA,axis) < pfxGetXYZMin(proxyB,axis)) {
+				break;
+			}
+
+			if(	pfxCheckCollidableInBroadphase(proxyA,proxyB) ) {
+				if(numPairs >= maxPairs) 
+					return SCE_PFX_ERR_OUT_OF_MAX_PAIRS;
+
+				PfxBroadphasePair &pair = pairs[numPairs++];
+				pfxSetActive(pair,true);
+				pfxSetObjectIdA(pair,pfxGetObjectId(proxyA));
+				pfxSetObjectIdB(pair,pfxGetObjectId(proxyB));
+				pfxSetMotionMaskA(pair,pfxGetMotionMask(proxyA));
+				pfxSetMotionMaskB(pair,pfxGetMotionMask(proxyB));
+				
+				pfxSetKey(pair,pfxCreateUniqueKey(pfxGetObjectId(proxyA),pfxGetObjectId(proxyB)));
+			}
+		}
+	}
+	
+	pfxSort(pairs,(PfxBroadphasePair*)workBuff,numPairs);
+	
+	result.pairs = pairs;
+	result.numPairs = numPairs;
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxDecomposePairs(PfxDecomposePairsParam &param,PfxDecomposePairsResult &result)
+{
+	PfxInt32 ret = pfxCheckParamOfDecomposePairs(param,0);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxDecomposePairs")
+
+	PfxBroadphasePair *previousPairs = param.previousPairs;
+	PfxUInt32 numPreviousPairs = param.numPreviousPairs;
+	PfxBroadphasePair *currentPairs = param.currentPairs;
+	PfxUInt32 numCurrentPairs = param.numCurrentPairs;
+	
+	PfxBroadphasePair *outNewPairs = (PfxBroadphasePair*)SCE_PFX_PTR_ALIGN16(param.pairBuff);
+	PfxBroadphasePair *outKeepPairs = outNewPairs + numCurrentPairs;
+	PfxBroadphasePair *outRemovePairs = outKeepPairs + numPreviousPairs;
+	
+	PfxUInt32 nNew = 0;
+	PfxUInt32 nKeep = 0;
+	PfxUInt32 nRemove = 0;
+	
+	PfxUInt32 oldId = 0,newId = 0;
+	
+	while(oldId<numPreviousPairs&&newId<numCurrentPairs) {
+		if(pfxGetKey(currentPairs[newId]) > pfxGetKey(previousPairs[oldId])) {
+			// remove
+			SCE_PFX_ASSERT(nRemove<=numPreviousPairs);
+			outRemovePairs[nRemove] = previousPairs[oldId];
+			nRemove++;
+			oldId++;
+		}
+		else if(pfxGetKey(currentPairs[newId]) == pfxGetKey(previousPairs[oldId])) {
+			// keep
+			SCE_PFX_ASSERT(nKeep<=numPreviousPairs);
+			outKeepPairs[nKeep] = currentPairs[newId];
+			pfxSetContactId(outKeepPairs[nKeep],pfxGetContactId(previousPairs[oldId]));
+			nKeep++;
+			oldId++;
+			newId++;
+		}
+		else {
+			// new
+			SCE_PFX_ASSERT(nNew<=numCurrentPairs);
+			outNewPairs[nNew] = currentPairs[newId];
+			nNew++;
+			newId++;
+		}
+	};
+	
+	if(newId<numCurrentPairs) {
+		// all new
+		for(;newId<numCurrentPairs;newId++,nNew++) {
+			SCE_PFX_ASSERT(nNew<=numCurrentPairs);
+			outNewPairs[nNew] = currentPairs[newId];
+		}
+	}
+	else if(oldId<numPreviousPairs) {
+		// all remove
+		for(;oldId<numPreviousPairs;oldId++,nRemove++) {
+			SCE_PFX_ASSERT(nRemove<=numPreviousPairs);
+			outRemovePairs[nRemove] = previousPairs[oldId];
+		}
+	}
+	
+	result.outNewPairs = outNewPairs;
+	result.outKeepPairs = outKeepPairs;
+	result.outRemovePairs = outRemovePairs;
+	result.numOutNewPairs = nNew;
+	result.numOutKeepPairs = nKeep;
+	result.numOutRemovePairs = nRemove;
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_batched_ray_cast_parallel.cpp

@@ -0,0 +1,95 @@
+/*
+Applied Research Associates Inc. (c)2011
+
+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/low_level/collision/pfx_batched_ray_cast.h"
+#include "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// MULTIPLE THREADS
+
+//----------------------------------------------------------------------------
+//  pfxCastRaysStartTaskEntry
+//
+/// The thread PfxTaskEntry function used to cast rays in parallel.
+//----------------------------------------------------------------------------
+void pfxCastRaysStartTaskEntry(PfxTaskArg *arg)
+{
+	PfxRayCastParam &param = *((PfxRayCastParam*)arg->io);
+
+	PfxRayInput *rayInputs = (PfxRayInput*)arg->data[0];
+	PfxRayOutput *rayOutputs = (PfxRayOutput*)arg->data[1];
+	PfxUInt32 iFirstRay = arg->data[2];
+	PfxUInt32 iEndRay = arg->data[3];
+
+	for(PfxUInt32 i = iFirstRay; i < iEndRay; i++)
+	{
+		pfxCastSingleRay(rayInputs[i], rayOutputs[i], param);
+	}
+}
+
+//----------------------------------------------------------------------------
+//  pfxCastRays
+//
+/// Perform cast rays in parallel using a task manager.
+///
+/// @param rayInputs    [in] Array of rays to cast
+/// @param rayOutputs   [out] On return contains output of ray casts
+/// @param param        Information about ray cast
+/// @param taskManager  Pointer to the thread task manager to use
+//----------------------------------------------------------------------------
+void pfxCastRays(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,
+	PfxRayCastParam &param,PfxTaskManager *taskManager)
+{
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables));
+
+	SCE_PFX_PUSH_MARKER("pfxCastRays");
+	
+	PfxUInt32 maxBatchSize = numRays / (PfxUInt32)(taskManager->getNumTasks());
+	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
+	int task = 0;
+	taskManager->setTaskEntry((void*)pfxCastRaysStartTaskEntry);
+
+	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), (PfxUInt32)rayInputs, (PfxUInt32)rayOutputs, iStart, iEnd);
+	}
+
+	// send final task
+	iEnd = numRays;
+	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	SCE_PFX_POP_MARKER();
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_batched_ray_cast_single.cpp

@@ -0,0 +1,47 @@
+/*
+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/low_level/collision/pfx_batched_ray_cast.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// SINGLE THREAD
+
+void pfxCastRaysStart(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,PfxRayCastParam &param)
+{
+	for(int i=0;i<numRays;i++) {
+		pfxCastSingleRay(rayInputs[i],rayOutputs[i],param);
+	}
+}
+
+void pfxCastRays(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,PfxRayCastParam &param)
+{
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates));
+	SCE_PFX_ALWAYS_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables));
+	
+	pfxCastRaysStart(rayInputs,rayOutputs,numRays,param);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_collision_detection_parallel.cpp

@@ -0,0 +1,166 @@
+/*
+Applied Research Associates Inc. (c)2011
+
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"
+#include "../../../include/physics_effects/low_level/collision/pfx_collision_detection.h"
+#include "../../base_level/broadphase/pfx_check_collidable.h"
+#include "../../base_level/collision/pfx_contact_cache.h"
+#include "pfx_detect_collision_func.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// This function is implemented in pfx_collision_detection_single.cpp
+extern int pfxCheckParamOfDetectCollision(PfxDetectCollisionParam &param);
+
+///////////////////////////////////////////////////////////////////////////////
+// MULTIPLE THREADS
+
+#define SCE_PFX_CONTACT_THRESHOLD 0.0f
+
+//----------------------------------------------------------------------------
+//  pfxDetectCollisionTaskEntry
+//
+/// The thread PfxTaskEntry function used to perform narrow phase collision
+/// detection in parallel.
+//----------------------------------------------------------------------------
+void pfxDetectCollisionTaskEntry(PfxTaskArg *arg)
+{
+	PfxDetectCollisionParam &param = *((PfxDetectCollisionParam*)arg->io);
+
+	PfxUInt32 iFirstContactPair = arg->data[0];
+	PfxUInt32 iEndContactPair = arg->data[1];
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxCollidable *offsetCollidables = param.offsetCollidables;
+	PfxUInt32 numRigidBodies = param.numRigidBodies;
+
+	for(PfxUInt32 i = iFirstContactPair; i < iEndContactPair; i++)
+	{
+		const PfxBroadphasePair &pair = contactPairs[i];
+		if(!pfxCheckCollidableInCollision(pair))
+			continue;
+
+		PfxUInt32 iContact = pfxGetContactId(pair);
+		PfxUInt32 iA = pfxGetObjectIdA(pair);
+		PfxUInt32 iB = pfxGetObjectIdB(pair);
+
+		PfxContactManifold &contact = offsetContactManifolds[iContact];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
+		SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
+
+		PfxRigidState &stateA = offsetRigidStates[iA];
+		PfxRigidState &stateB = offsetRigidStates[iB];
+		PfxCollidable &collA = offsetCollidables[iA];
+		PfxCollidable &collB = offsetCollidables[iB];
+		PfxTransform3 tA0(stateA.getOrientation(), stateA.getPosition());
+		PfxTransform3 tB0(stateB.getOrientation(), stateB.getPosition());
+		
+		PfxContactCache contactCache;
+		
+		PfxShapeIterator itrShapeA(collA);
+		for(PfxUInt32 j=0;j<collA.getNumShapes();j++,++itrShapeA)
+		{
+			const PfxShape &shapeA = *itrShapeA;
+			PfxTransform3 offsetTrA = shapeA.getOffsetTransform();
+			PfxTransform3 worldTrA = tA0 * offsetTrA;
+
+			PfxShapeIterator itrShapeB(collB);
+			for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB)
+			{
+				const PfxShape &shapeB = *itrShapeB;
+				PfxTransform3 offsetTrB = shapeB.getOffsetTransform();
+				PfxTransform3 worldTrB = tB0 * offsetTrB;
+
+				if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) && 
+				    (shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) )
+				{
+					pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(
+						contactCache,
+						shapeA,offsetTrA,worldTrA,j,
+						shapeB,offsetTrB,worldTrB,k,
+						SCE_PFX_CONTACT_THRESHOLD);
+				}
+			}
+		}
+		
+		for(int j=0;j<contactCache.getNumContacts();j++)
+		{
+			const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);
+
+			contact.addContactPoint(
+				cp.m_distance,
+				cp.m_normal,
+				cp.m_localPointA,
+				cp.m_localPointB,
+				cp.m_subData
+				);
+		}
+	}
+}
+
+//----------------------------------------------------------------------------
+//  pfxDetectCollision
+//
+/// Perform narrow phase collision detection in parallel using a task
+/// manager.
+///
+/// @param param        Information about pairs that may be colliding
+/// @param taskManager  Pointer to the thread task manager to use
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param, PfxTaskManager *taskManager)
+{
+	PfxInt32 ret = pfxCheckParamOfDetectCollision(param);
+	if(ret != SCE_PFX_OK) 
+		return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxDetectCollision");
+
+	PfxUInt32 maxBatchSize = param.numContactPairs / (PfxUInt32)(taskManager->getNumTasks());
+	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
+	int task = 0;
+	taskManager->setTaskEntry((void*)pfxDetectCollisionTaskEntry);
+
+	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+	}
+
+	// send final task
+	iEnd = param.numContactPairs;
+	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	SCE_PFX_POP_MARKER();
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_collision_detection_single.cpp

@@ -0,0 +1,125 @@
+/*
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"
+#include "../../../include/physics_effects/low_level/collision/pfx_collision_detection.h"
+#include "../../base_level/broadphase/pfx_check_collidable.h"
+#include "../../base_level/collision/pfx_contact_cache.h"
+#include "pfx_detect_collision_func.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+
+int pfxCheckParamOfDetectCollision(PfxDetectCollisionParam &param)
+{
+	if(!param.contactPairs || !param.offsetContactManifolds || !param.offsetRigidStates|| !param.offsetCollidables ) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || 
+		!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables)) return SCE_PFX_ERR_INVALID_ALIGN;
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// SINGLE THREAD
+
+#define SCE_PFX_CONTACT_THRESHOLD 0.0f
+
+PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param)
+{
+	PfxInt32 ret = pfxCheckParamOfDetectCollision(param);
+	if(ret != SCE_PFX_OK) 
+		return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxDetectCollision");
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxUInt32 numContactPairs = param.numContactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxCollidable *offsetCollidables = param.offsetCollidables;
+	PfxUInt32 numRigidBodies = param.numRigidBodies;
+
+	for(PfxUInt32 i=0;i<numContactPairs;i++) {
+		const PfxBroadphasePair &pair = contactPairs[i];
+		if(!pfxCheckCollidableInCollision(pair)) {
+			continue;
+		}
+
+		PfxUInt32 iContact = pfxGetContactId(pair);
+		PfxUInt32 iA = pfxGetObjectIdA(pair);
+		PfxUInt32 iB = pfxGetObjectIdB(pair);
+
+		PfxContactManifold &contact = offsetContactManifolds[iContact];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
+		SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
+
+		PfxRigidState &stateA = offsetRigidStates[iA];
+		PfxRigidState &stateB = offsetRigidStates[iB];
+		PfxCollidable &collA = offsetCollidables[iA];
+		PfxCollidable &collB = offsetCollidables[iB];
+		PfxTransform3 tA0(stateA.getOrientation(), stateA.getPosition());
+		PfxTransform3 tB0(stateB.getOrientation(), stateB.getPosition());
+		
+		PfxContactCache contactCache;
+		
+		PfxShapeIterator itrShapeA(collA);
+		for(PfxUInt32 j=0;j<collA.getNumShapes();j++,++itrShapeA) {
+			const PfxShape &shapeA = *itrShapeA;
+			PfxTransform3 offsetTrA = shapeA.getOffsetTransform();
+			PfxTransform3 worldTrA = tA0 * offsetTrA;
+
+			PfxShapeIterator itrShapeB(collB);
+			for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB) {
+				const PfxShape &shapeB = *itrShapeB;
+				PfxTransform3 offsetTrB = shapeB.getOffsetTransform();
+				PfxTransform3 worldTrB = tB0 * offsetTrB;
+
+				if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) && 
+				    (shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) ) {
+					pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(
+						contactCache,
+						shapeA,offsetTrA,worldTrA,j,
+						shapeB,offsetTrB,worldTrB,k,
+						SCE_PFX_CONTACT_THRESHOLD);
+				}
+			}
+		}
+		
+		for(int j=0;j<contactCache.getNumContacts();j++) {
+			const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);
+
+			contact.addContactPoint(
+				cp.m_distance,
+				cp.m_normal,
+				cp.m_localPointA,
+				cp.m_localPointB,
+				cp.m_subData
+				);
+		}
+	}
+
+	SCE_PFX_POP_MARKER();
+	
+	(void) numRigidBodies;
+
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_detect_collision_func.cpp

@@ -0,0 +1,449 @@
+/*
+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/collision/pfx_shape.h"
+#include "../../base_level/collision/pfx_contact_box_box.h"
+#include "../../base_level/collision/pfx_contact_box_capsule.h"
+#include "../../base_level/collision/pfx_contact_box_sphere.h"
+#include "../../base_level/collision/pfx_contact_capsule_capsule.h"
+#include "../../base_level/collision/pfx_contact_capsule_sphere.h"
+#include "../../base_level/collision/pfx_contact_sphere_sphere.h"
+#include "../../base_level/collision/pfx_gjk_solver.h"
+#include "../../base_level/collision/pfx_contact_large_tri_mesh.h"
+#include "../../base_level/collision/pfx_gjk_support_func.h"
+#include "pfx_detect_collision_func.h"
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Collision Detection Function
+
+void detectCollisionDummy(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)contacts;
+	(void)shapeA,(void)offsetTransformA,(void)worldTransformA,(void)shapeIdA;
+	(void)shapeB,(void)offsetTransformB,(void)worldTransformB,(void)shapeIdB;
+	(void)contactThreshold;
+}
+
+void detectCollisionBoxBox(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxBox boxA = shapeA.getBox();
+	PfxBox boxB = shapeB.getBox();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactBoxBox(nml,pA,pB,&boxA,worldTransformA,&boxB,worldTransformB);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionBoxCapsule(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxBox boxA = shapeA.getBox();
+	PfxCapsule capsuleB = shapeB.getCapsule();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactBoxCapsule(nml,pA,pB,&boxA,worldTransformA,&capsuleB,worldTransformB);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionBoxSphere(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxBox boxA = shapeA.getBox();
+	PfxSphere sphereB = shapeB.getSphere();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactBoxSphere(nml,pA,pB,&boxA,worldTransformA,&sphereB,worldTransformB);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionCapsuleBox(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxCapsule capsuleA = shapeA.getCapsule();
+	PfxBox boxB = shapeB.getBox();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactBoxCapsule(nml,pB,pA,&boxB,worldTransformB,&capsuleA,worldTransformA);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionCapsuleCapsule(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxCapsule capsuleA = shapeA.getCapsule();
+	PfxCapsule capsuleB = shapeB.getCapsule();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactCapsuleCapsule(nml,pA,pB,&capsuleA,worldTransformA,&capsuleB,worldTransformB);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionCapsuleSphere(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxCapsule capsuleA = shapeA.getCapsule();
+	PfxSphere sphereB = shapeB.getSphere();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactCapsuleSphere(nml,pA,pB,&capsuleA,worldTransformA,&sphereB,worldTransformB);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionSphereBox(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxSphere sphereA = shapeA.getSphere();
+	PfxBox boxB = shapeB.getBox();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactBoxSphere(nml,pB,pA,&boxB,worldTransformB,&sphereA,worldTransformA);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionSphereCapsule(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxSphere sphereA = shapeA.getSphere();
+	PfxCapsule capsuleB = shapeB.getCapsule();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactCapsuleSphere(nml,pB,pA,&capsuleB,worldTransformB,&sphereA,worldTransformA);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionSphereSphere(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxSphere sphereA = shapeA.getSphere();
+	PfxSphere sphereB = shapeB.getSphere();
+	
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	
+	PfxFloat d = pfxContactSphereSphere(nml,pA,pB,&sphereA,worldTransformA,&sphereB,worldTransformB);
+	
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,-nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionGjk(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	PfxFloat d = SCE_PFX_FLT_MAX;
+	PfxVector3 nml;
+	PfxPoint3 pA,pB;
+	PfxGjkSolver gjk;
+
+	if(shapeA.getType() == kPfxShapeCylinder) {
+		PfxCylinder cylinderA = shapeA.getCylinder();
+		
+		if(shapeB.getType() == kPfxShapeSphere) {
+			PfxSphere sphereB = shapeB.getSphere();
+			gjk.setup((void*)&cylinderA,(void*)&sphereB,pfxGetSupportVertexCylinder,pfxGetSupportVertexSphere);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeBox) {
+			PfxBox boxB = shapeB.getBox();
+			gjk.setup((void*)&cylinderA,(void*)&boxB,pfxGetSupportVertexCylinder,pfxGetSupportVertexBox);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeCapsule) {
+			PfxCapsule capsuleB = shapeB.getCapsule();
+			gjk.setup((void*)&cylinderA,(void*)&capsuleB,pfxGetSupportVertexCylinder,pfxGetSupportVertexCapsule);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeCylinder) {
+			PfxCylinder cylinderB = shapeB.getCylinder();
+			gjk.setup((void*)&cylinderA,(void*)&cylinderB,pfxGetSupportVertexCylinder,pfxGetSupportVertexCylinder);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeConvexMesh) {
+		const PfxConvexMesh *convexB = shapeB.getConvexMesh();
+
+			gjk.setup((void*)&cylinderA,(void*)convexB,pfxGetSupportVertexCylinder,pfxGetSupportVertexConvex);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+
+		}
+	}
+	else if(shapeB.getType() == kPfxShapeCylinder) {
+		PfxCylinder cylinderB = shapeB.getCylinder();
+		
+		if(shapeA.getType() == kPfxShapeSphere) {
+			PfxSphere sphereA = shapeA.getSphere();
+			gjk.setup((void*)&sphereA,(void*)&cylinderB,pfxGetSupportVertexSphere,pfxGetSupportVertexCylinder);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeA.getType() == kPfxShapeBox) {
+			PfxBox boxA = shapeA.getBox();
+			gjk.setup((void*)&boxA,(void*)&cylinderB,pfxGetSupportVertexBox,pfxGetSupportVertexCylinder);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeA.getType() == kPfxShapeCapsule) {
+			PfxCapsule capsuleA = shapeA.getCapsule();
+			gjk.setup((void*)&capsuleA,(void*)&cylinderB,pfxGetSupportVertexCapsule,pfxGetSupportVertexCylinder);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeA.getType() == kPfxShapeConvexMesh) {
+		const PfxConvexMesh *convexA = shapeA.getConvexMesh();
+
+			gjk.setup((void*)convexA,(void*)&cylinderB,pfxGetSupportVertexConvex,pfxGetSupportVertexCylinder);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+
+		}
+	}
+	else if(shapeA.getType() == kPfxShapeConvexMesh) {
+	const PfxConvexMesh *convexA = shapeA.getConvexMesh();
+		
+		if(shapeB.getType() == kPfxShapeSphere) {
+			PfxSphere sphereB = shapeB.getSphere();
+			gjk.setup((void*)convexA,(void*)&sphereB,pfxGetSupportVertexConvex,pfxGetSupportVertexSphere);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeBox) {
+			PfxBox boxB = shapeB.getBox();
+			gjk.setup((void*)convexA,(void*)&boxB,pfxGetSupportVertexConvex,pfxGetSupportVertexBox);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeCapsule) {
+			PfxCapsule capsuleB = shapeB.getCapsule();
+			gjk.setup((void*)convexA,(void*)&capsuleB,pfxGetSupportVertexConvex,pfxGetSupportVertexCapsule);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeCylinder) {
+			PfxCylinder cylinderB = shapeB.getCylinder();
+			gjk.setup((void*)convexA,(void*)&cylinderB,pfxGetSupportVertexConvex,pfxGetSupportVertexCylinder);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeB.getType() == kPfxShapeConvexMesh) {
+		const PfxConvexMesh *convexB = shapeB.getConvexMesh();
+
+			gjk.setup((void*)convexA,(void*)convexB,pfxGetSupportVertexConvex,pfxGetSupportVertexConvex);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+
+		}
+		
+	}
+	else if(shapeB.getType() == kPfxShapeConvexMesh) {
+	const PfxConvexMesh *convexB = shapeB.getConvexMesh();
+		
+		if(shapeA.getType() == kPfxShapeSphere) {
+			PfxSphere sphereA = shapeA.getSphere();
+			gjk.setup((void*)&sphereA,(void*)convexB,pfxGetSupportVertexSphere,pfxGetSupportVertexConvex);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeA.getType() == kPfxShapeBox) {
+			PfxBox boxA = shapeA.getBox();
+			gjk.setup((void*)&boxA,(void*)convexB,pfxGetSupportVertexBox,pfxGetSupportVertexConvex);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeA.getType() == kPfxShapeCapsule) {
+			PfxCapsule capsuleA = shapeA.getCapsule();
+			gjk.setup((void*)&capsuleA,(void*)convexB,pfxGetSupportVertexCapsule,pfxGetSupportVertexConvex);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+		}
+		else if(shapeA.getType() == kPfxShapeConvexMesh) {
+		const PfxConvexMesh *convexA = shapeA.getConvexMesh();
+			
+			gjk.setup((void*)convexA,(void*)convexB,pfxGetSupportVertexConvex,pfxGetSupportVertexConvex);
+			d = gjk.collide(nml,pA,pB,worldTransformA,worldTransformB,SCE_PFX_FLT_MAX);
+
+		}
+
+	}
+
+	if(d < contactThreshold) {
+		contacts.addContactPoint(d,nml,offsetTransformA*pA,offsetTransformB*pB,PfxSubData());
+	}
+}
+
+void detectCollisionLargeTriMesh(
+				PfxContactCache &contacts,
+				const PfxShape &shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape &shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold)
+{
+	(void)shapeIdA,(void)shapeIdB;
+	if(shapeA.getType() == kPfxShapeLargeTriMesh) {
+	const PfxLargeTriMesh *lmeshA = shapeA.getLargeTriMesh();
+		
+		PfxContactCache localContacts;
+		pfxContactLargeTriMesh(localContacts,
+			lmeshA,worldTransformA,
+			shapeB,worldTransformB,
+			contactThreshold);
+		
+		
+		for(int i=0;i<localContacts.getNumContacts();i++) {
+			contacts.addContactPoint(
+				localContacts.getDistance(i),
+				localContacts.getNormal(i),
+				offsetTransformA * localContacts.getLocalPointA(i),
+				offsetTransformB * localContacts.getLocalPointB(i),
+				localContacts.getSubData(i));
+		}
+	}
+	else if(shapeB.getType() == kPfxShapeLargeTriMesh) {
+	const PfxLargeTriMesh *lmeshB = shapeB.getLargeTriMesh();
+
+		PfxContactCache localContacts;
+		pfxContactLargeTriMesh(localContacts,
+			lmeshB,worldTransformB,
+			shapeA,worldTransformA,
+			contactThreshold);
+		
+		
+		for(int i=0;i<localContacts.getNumContacts();i++) {
+			contacts.addContactPoint(
+				localContacts.getDistance(i),
+				-localContacts.getNormal(i),
+				offsetTransformA * localContacts.getLocalPointB(i),
+				offsetTransformB * localContacts.getLocalPointA(i),
+				localContacts.getSubData(i));
+		}
+	}
+}
+///////////////////////////////////////////////////////////////////////////////
+// Collision Detection Function Table
+
+pfx_detect_collision_func funcTbl_detectCollision[kPfxShapeCount][kPfxShapeCount] = {
+	{detectCollisionSphereSphere	,detectCollisionSphereBox	,detectCollisionSphereCapsule	,detectCollisionGjk			,detectCollisionGjk				,detectCollisionLargeTriMesh,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionBoxSphere		,detectCollisionBoxBox		,detectCollisionBoxCapsule		,detectCollisionGjk			,detectCollisionGjk				,detectCollisionLargeTriMesh,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionCapsuleSphere	,detectCollisionCapsuleBox	,detectCollisionCapsuleCapsule	,detectCollisionGjk			,detectCollisionGjk				,detectCollisionLargeTriMesh,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionGjk				,detectCollisionGjk			,detectCollisionGjk				,detectCollisionGjk			,detectCollisionGjk				,detectCollisionLargeTriMesh,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionGjk				,detectCollisionGjk			,detectCollisionGjk				,detectCollisionGjk			,detectCollisionGjk				,detectCollisionLargeTriMesh,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionLargeTriMesh	,detectCollisionLargeTriMesh,detectCollisionLargeTriMesh	,detectCollisionLargeTriMesh,detectCollisionLargeTriMesh	,detectCollisionDummy,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+	{detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy	,detectCollisionDummy,	detectCollisionDummy},
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Collision Detection Function Table Interface
+
+pfx_detect_collision_func pfxGetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB)
+{
+	SCE_PFX_ASSERT(shapeTypeA<kPfxShapeCount);
+	SCE_PFX_ASSERT(shapeTypeB<kPfxShapeCount);
+	return funcTbl_detectCollision[shapeTypeA][shapeTypeB];
+}
+
+int pfxSetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB,pfx_detect_collision_func func)
+{
+	if(shapeTypeA >= kPfxShapeCount || shapeTypeB >= kPfxShapeCount) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	
+	funcTbl_detectCollision[shapeTypeA][shapeTypeB] = func;
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_detect_collision_func.h

@@ -0,0 +1,37 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_DETECT_COLLISION_FUNC_H
+#define _SCE_PFX_DETECT_COLLISION_FUNC_H
+
+#include "../../base_level/collision/pfx_contact_cache.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+typedef void (*pfx_detect_collision_func)(
+				PfxContactCache &contacts,
+				const PfxShape & shapeA,const PfxTransform3 &offsetTransformA,const PfxTransform3 &worldTransformA,int shapeIdA,
+				const PfxShape & shapeB,const PfxTransform3 &offsetTransformB,const PfxTransform3 &worldTransformB,int shapeIdB,
+				float contactThreshold);
+
+pfx_detect_collision_func pfxGetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB);
+
+int pfxSetDetectCollisionFunc(PfxUInt8 shapeTypeA,PfxUInt8 shapeTypeB,pfx_detect_collision_func func);
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_DETECT_COLLISION_FUNC_H

Extras/PhysicsEffects/src/low_level/collision/pfx_intersect_ray_func.cpp

@@ -0,0 +1,129 @@
+/*
+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/collision/pfx_shape.h"
+#include "../../base_level/collision/pfx_intersect_ray_box.h"
+#include "../../base_level/collision/pfx_intersect_ray_sphere.h"
+#include "../../base_level/collision/pfx_intersect_ray_capsule.h"
+#include "../../base_level/collision/pfx_intersect_ray_cylinder.h"
+#include "../../base_level/collision/pfx_intersect_ray_convex.h"
+#include "../../base_level/collision/pfx_intersect_ray_large_tri_mesh.h"
+#include "pfx_intersect_ray_func.h"
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Ray Intersection Function Table
+
+PfxBool intersectRayFuncDummy(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform)
+{
+	(void)ray,(void)out,(void)shape,(void)transform;
+	return false;
+}
+
+PfxBool intersectRayFuncBox(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform)
+{
+	return pfxIntersectRayBox(ray,out,shape.getBox(),transform);
+}
+
+PfxBool intersectRayFuncSphere(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform)
+{
+	return pfxIntersectRaySphere(ray,out,shape.getSphere(),transform);
+}
+
+PfxBool intersectRayFuncCapsule(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform)
+{
+	return pfxIntersectRayCapsule(ray,out,shape.getCapsule(),transform);
+}
+
+PfxBool intersectRayFuncCylinder(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform)
+{
+	return pfxIntersectRayCylinder(ray,out,shape.getCylinder(),transform);
+}
+
+PfxBool intersectRayFuncConvex(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform)
+{
+const PfxConvexMesh *convex = shape.getConvexMesh();
+	
+	PfxBool ret = pfxIntersectRayConvex(ray,out,(const void*)convex,transform);
+	
+	
+	return ret;
+}
+
+PfxBool intersectRayFuncLargeTriMesh(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform)
+{
+const PfxLargeTriMesh *lmesh = shape.getLargeTriMesh();
+	
+	PfxBool ret = pfxIntersectRayLargeTriMesh(ray,out,(const void*)lmesh,transform);
+	
+	
+	return ret;
+}
+
+PfxIntersectRayFunc funcTbl_intersectRay[kPfxShapeCount] = {
+	intersectRayFuncSphere,
+	intersectRayFuncBox,
+	intersectRayFuncCapsule,
+	intersectRayFuncCylinder,
+	intersectRayFuncConvex,
+	intersectRayFuncLargeTriMesh,
+	intersectRayFuncDummy,
+	intersectRayFuncDummy,
+	intersectRayFuncDummy,
+	intersectRayFuncDummy,
+	intersectRayFuncDummy,
+	intersectRayFuncDummy,
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Ray Intersection Function Table Interface
+
+PfxIntersectRayFunc pfxGetIntersectRayFunc(PfxUInt8 shapeType)
+{
+	return funcTbl_intersectRay[shapeType];
+}
+
+PfxInt32 pfxSetIntersectRayFunc(PfxUInt8 shapeType,PfxIntersectRayFunc func)
+{
+	if(shapeType >= kPfxShapeCount) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	
+	funcTbl_intersectRay[shapeType] = func;
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_intersect_ray_func.h

@@ -0,0 +1,36 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_INTERSECT_RAY_FUNC_H
+#define _SCE_PFX_INTERSECT_RAY_FUNC_H
+
+#include "../../../include/physics_effects/base_level/collision/pfx_ray.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+typedef PfxBool (*PfxIntersectRayFunc)(
+				const PfxRayInput &ray,PfxRayOutput &out,
+				const PfxShape &shape,const PfxTransform3 &transform);
+
+PfxIntersectRayFunc pfxGetIntersectRayFunc(PfxUInt8 shapeType);
+
+PfxInt32 pfxSetIntersectRayFunc(PfxUInt8 shapeType,PfxIntersectRayFunc func);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif /* _SCE_PFX_INTERSECT_RAY_FUNC_H */

Extras/PhysicsEffects/src/low_level/collision/pfx_island_generation.cpp

@@ -0,0 +1,231 @@
+/*
+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/rigidbody/pfx_rigid_state.h"
+#include "../../../include/physics_effects/low_level/collision/pfx_island_generation.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxIslandNode {
+	PfxUInt32 rootId;
+	PfxUInt32 rank;
+	PfxUInt32 islandId;
+	PfxUInt32 isRoot;
+};
+
+struct PfxIslandUnit
+{
+	PfxUInt32 id;
+	PfxIslandUnit *next;
+};
+
+struct PfxIsland
+{
+	PfxUInt32 numNodes;
+	PfxUInt32 numIslands;
+	PfxIslandNode *nodes;
+	PfxIslandUnit *islandsUnits;
+	PfxIslandUnit **islandsHeads;
+};
+
+PfxUInt32 pfxIslandNodeFind(PfxUInt32 i,PfxIsland *island)
+{
+	if( i != island->nodes[i].rootId ) {
+		island->nodes[i].rootId = pfxIslandNodeFind(island->nodes[i].rootId,island);
+	}
+	return island->nodes[i].rootId;
+}
+
+void pfxIslandNodeLink(PfxUInt32 iA,PfxUInt32 iB,PfxIsland *island)
+{
+	if(island->nodes[iA].rank > island->nodes[iB].rank) {
+		island->nodes[iB].rootId = iA;
+	}
+	else if(island->nodes[iA].rank == island->nodes[iB].rank) {
+		island->nodes[iA].rootId = iB;
+		island->nodes[iB].rank++;
+	}
+	else {
+		island->nodes[iA].rootId = iB;
+	}
+}
+
+void pfxIslandNodeUnion(PfxUInt32 iA,PfxUInt32 iB,PfxIsland *island)
+{
+	SCE_PFX_ALWAYS_ASSERT(iA<island->numNodes);
+	SCE_PFX_ALWAYS_ASSERT(iB<island->numNodes);
+	
+	pfxIslandNodeLink(pfxIslandNodeFind(iA,island),pfxIslandNodeFind(iB,island),island);
+}
+
+PfxUInt32 pfxGetIslandBytesOfGenerateIsland(PfxUInt32 numObjects)
+{
+	return 16 + 
+		SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIsland)) + 
+		SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIslandNode)*numObjects) + 
+		SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIslandUnit*)*numObjects) +
+		SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxIslandUnit)*numObjects);
+}
+
+SCE_PFX_FORCE_INLINE int pfxCheckParamOfGenerateIsland(const PfxGenerateIslandParam &param)
+{
+	if(!param.islandBuff || !param.pairs) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.pairs)) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.islandBuff,param.islandBytes) < pfxGetIslandBytesOfGenerateIsland(param.numObjects) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxGenerateIsland(PfxGenerateIslandParam &param,PfxGenerateIslandResult &result)
+{
+	int ret = pfxCheckParamOfGenerateIsland(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	PfxConstraintPair *pairs = param.pairs;
+	PfxUInt32 numPairs = param.numPairs;
+	PfxUInt32 numUnits = param.numObjects;
+
+	memset(param.islandBuff,0,param.islandBytes);
+
+	PfxHeapManager pool((unsigned char*)param.islandBuff,param.islandBytes);
+
+	PfxIsland *island = (PfxIsland*)pool.allocate(sizeof(PfxIsland));
+	island->numIslands = 0;
+	island->numNodes = numUnits;
+	island->nodes = (PfxIslandNode*)pool.allocate(sizeof(PfxIslandNode)*numUnits);
+	island->islandsHeads = (PfxIslandUnit**)pool.allocate(sizeof(PfxIslandUnit*)*numUnits);
+	island->islandsUnits = (PfxIslandUnit*)pool.allocate(sizeof(PfxIslandUnit)*numUnits);
+	
+	result.island = island;
+	
+	// 初期化
+	for(PfxUInt32 i=0;i<island->numNodes;i++) {
+		island->nodes[i].rootId = i;
+		island->nodes[i].rank = 0;
+	}
+	
+	return pfxAppendPairs(island,pairs,numPairs);
+}
+
+PfxUInt32 pfxGetNumIslands(const PfxIsland *islands)
+{
+	SCE_PFX_ALWAYS_ASSERT(islands);
+	return islands->numIslands;
+}
+
+PfxIslandUnit *pfxGetFirstUnitInIsland(const PfxIsland *islands,PfxUInt32 islandId)
+{
+	SCE_PFX_ALWAYS_ASSERT(islands);
+	SCE_PFX_ALWAYS_ASSERT(islandId < islands->numIslands);
+	return islands->islandsHeads[islandId];
+}
+
+PfxIslandUnit *pfxGetNextUnitInIsland(const PfxIslandUnit *islandUnit)
+{
+	SCE_PFX_ALWAYS_ASSERT(islandUnit);
+	return islandUnit->next;
+}
+
+PfxUInt32 pfxGetUnitId(const PfxIslandUnit *islandUnit)
+{
+	SCE_PFX_ALWAYS_ASSERT(islandUnit);
+	return islandUnit->id;
+}
+
+PfxUInt32 pfxGetIslandId(const PfxIsland *islands,PfxUInt32 unitId)
+{
+	SCE_PFX_ALWAYS_ASSERT(islands&&unitId<islands->numNodes);
+	return islands->nodes[unitId].islandId;
+}
+
+PfxInt32 pfxAppendPairs(PfxIsland *island,PfxConstraintPair *pairs,PfxUInt32 numPairs)
+{
+	if(numPairs == 0) {
+		return SCE_PFX_OK;
+	}
+
+	if(!island || !pairs) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(island) || !SCE_PFX_PTR_IS_ALIGNED16(pairs)) return SCE_PFX_ERR_INVALID_ALIGN;
+
+	// 統合
+	for(PfxUInt32 i=0;i<numPairs;i++) {
+		PfxConstraintPair &pair = pairs[i];
+		if(pfxGetActive(pair)) {
+			PfxUInt32 iA = pfxGetObjectIdA(pair);
+			PfxUInt32 iB = pfxGetObjectIdB(pair);
+			
+			SCE_PFX_ALWAYS_ASSERT(iA<island->numNodes);
+			SCE_PFX_ALWAYS_ASSERT(iB<island->numNodes);
+			
+			if( (SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskA(pair)&SCE_PFX_MOTION_MASK_TYPE)) && 
+				(SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskB(pair)&SCE_PFX_MOTION_MASK_TYPE)) ) {
+				pfxIslandNodeUnion(iA,iB,island);
+			}
+		}
+	}
+
+	// アイランド生成のための初期化
+	for(PfxUInt32 i=0;i<island->numNodes;i++) {
+		island->nodes[i].islandId = 0;
+		island->nodes[i].isRoot = 0;
+		island->islandsHeads[i] = NULL;
+	}
+
+	// 親へ直結
+	PfxUInt32 id = 0;
+	for(PfxUInt32 i=0;i<island->numNodes;i++) {
+		PfxUInt32 rootId = pfxIslandNodeFind(i,island);
+		if( island->nodes[rootId].isRoot == 0 ) {
+			island->nodes[rootId].islandId = id++;
+			island->nodes[rootId].isRoot = 1;
+		}
+		island->nodes[i].islandId = island->nodes[rootId].islandId;
+	}
+
+	// アイランド作成
+	PfxUInt32 n = 0;
+	for(PfxUInt32 i=0;i<island->numNodes;i++) {
+		PfxUInt32 islandId = island->nodes[i].islandId;
+		PfxIslandUnit *newUnit = &island->islandsUnits[n++];
+		newUnit->id = i;
+
+		if(!island->islandsHeads[islandId]) {
+			island->islandsHeads[islandId] = newUnit;
+			continue;
+		}
+		
+		PfxIslandUnit *unit=island->islandsHeads[islandId];
+		island->islandsHeads[islandId] = newUnit;
+		newUnit->next = unit;
+	}
+	
+	island->numIslands = id;
+	
+	return SCE_PFX_OK;
+}
+
+void pfxResetIsland(PfxIsland *island)
+{
+	SCE_PFX_ALWAYS_ASSERT(island);
+	island->numIslands = 0;
+	for(PfxUInt32 i=0;i<island->numNodes;i++) {
+		island->nodes[i].rootId = i;
+		island->nodes[i].rank = 0;
+	}
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_ray_cast.cpp

@@ -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 &center,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 &center,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 &param)
+{
+	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

Extras/PhysicsEffects/src/low_level/collision/pfx_refresh_contacts_parallel.cpp

@@ -0,0 +1,115 @@
+/*
+Applied Research Associates Inc. (c)2011
+
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
+#include "../../../include/physics_effects/low_level/collision/pfx_refresh_contacts.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// This function is implemented in pfx_refresh_contacts_single.cpp
+extern int pfxCheckParamOfRefreshContacts(PfxRefreshContactsParam &param);
+
+///////////////////////////////////////////////////////////////////////////////
+// MULTIPLE THREADS
+
+//----------------------------------------------------------------------------
+//  pfxRefreshContactsTaskEntry
+//
+/// The thread PfxTaskEntry function used to perform refresh contacts in
+/// parallel
+//----------------------------------------------------------------------------
+void pfxRefreshContactsTaskEntry(PfxTaskArg *arg)
+{
+	PfxRefreshContactsParam &param = *((PfxRefreshContactsParam*)arg->io);
+	
+	PfxUInt32 iFirstContactPair = arg->data[0];
+	PfxUInt32 iEndContactPair = arg->data[1];
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxUInt32 numRigidBodies = param.numRigidBodies;
+	
+	for(PfxUInt32 i = iFirstContactPair; i < iEndContactPair; i++)
+	{
+		PfxBroadphasePair &pair = contactPairs[i];
+		
+		PfxUInt32 iContact = pfxGetContactId(pair);
+		PfxUInt32 iA = pfxGetObjectIdA(pair);
+		PfxUInt32 iB = pfxGetObjectIdB(pair);
+
+		PfxContactManifold &contact = offsetContactManifolds[iContact];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
+		SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
+
+		PfxRigidState &instA = offsetRigidStates[iA];
+		PfxRigidState &instB = offsetRigidStates[iB];
+		
+		contact.refresh(
+			instA.getPosition(),instA.getOrientation(),
+			instB.getPosition(),instB.getOrientation() );
+	}
+}
+
+//----------------------------------------------------------------------------
+//  pfxRefreshContacts
+//
+/// Perform refresh contacts in parallel using a task manager.
+///
+/// @param param        Information about contact pairs
+/// @param taskManager  Pointer to the thread task manager to use
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+PfxInt32 pfxRefreshContacts(PfxRefreshContactsParam &param, PfxTaskManager *taskManager)
+{
+	PfxInt32 ret = pfxCheckParamOfRefreshContacts(param);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxRefreshContacts");
+
+	PfxUInt32 maxBatchSize = param.numContactPairs / (PfxUInt32)(taskManager->getNumTasks());
+	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
+	int task = 0;
+	taskManager->setTaskEntry((void*)pfxRefreshContactsTaskEntry);
+
+	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+	}
+
+	// send final task
+	iEnd = param.numContactPairs;
+	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	SCE_PFX_POP_MARKER();
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/collision/pfx_refresh_contacts_single.cpp

@@ -0,0 +1,76 @@
+/*
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
+#include "../../../include/physics_effects/low_level/collision/pfx_refresh_contacts.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+int pfxCheckParamOfRefreshContacts(PfxRefreshContactsParam &param)
+{
+	if(!param.contactPairs || !param.offsetContactManifolds || !param.offsetRigidStates ) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || 
+		!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates)) return SCE_PFX_ERR_INVALID_ALIGN;
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// SINGLE THREAD
+
+PfxInt32 pfxRefreshContacts(PfxRefreshContactsParam &param)
+{
+	PfxInt32 ret = pfxCheckParamOfRefreshContacts(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	SCE_PFX_PUSH_MARKER("pfxRefreshContacts");
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxUInt32 numContactPairs = param.numContactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxUInt32 numRigidBodies = param.numRigidBodies;
+	
+	for(PfxUInt32 i=0;i<numContactPairs;i++) {
+		PfxBroadphasePair &pair = contactPairs[i];
+		
+		PfxUInt32 iContact = pfxGetContactId(pair);
+		PfxUInt32 iA = pfxGetObjectIdA(pair);
+		PfxUInt32 iB = pfxGetObjectIdB(pair);
+
+		PfxContactManifold &contact = offsetContactManifolds[iContact];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
+		SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
+
+		PfxRigidState &instA = offsetRigidStates[iA];
+		PfxRigidState &instB = offsetRigidStates[iB];
+		
+		contact.refresh(
+			instA.getPosition(),instA.getOrientation(),
+			instB.getPosition(),instB.getOrientation() );
+	}
+
+	SCE_PFX_POP_MARKER();
+
+	(void) numRigidBodies;
+
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/premake4.lua

@@ -0,0 +1,12 @@
+	project "physicseffects2_lowlevel"
+		
+	kind "StaticLib"
+	targetdir "../../build/lib"	
+	includedirs {
+		".",
+	}
+	files {
+		"**.cpp",
+		"../../include/physics_effects/low_level/**.h"
+
+	}

Extras/PhysicsEffects/src/low_level/solver/pfx_constraint_solver_parallel.cpp

@@ -0,0 +1,795 @@
+/*
+Applied Research Associates Inc. (c)2011
+
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_contact_constraint.h"
+#include "../../../include/physics_effects/low_level/solver/pfx_joint_constraint_func.h"
+#include "../../../include/physics_effects/low_level/solver/pfx_constraint_solver.h"
+#include "../../base_level/solver/pfx_check_solver.h"
+#include "pfx_parallel_group.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// These functions are implemented in pfx_constraint_solver_single.cpp
+extern PfxUInt32 pfxGetWorkBytesOfSolveConstraints(PfxUInt32 numRigidBodies,PfxUInt32 numContactPairs,PfxUInt32 numJointPairs,PfxUInt32 maxTasks);
+extern PfxInt32 pfxCheckParamOfSetupSolverBodies(const PfxSetupSolverBodiesParam &param);
+extern PfxInt32 pfxCheckParamOfSetupContactConstraints(const PfxSetupContactConstraintsParam &param);
+extern PfxInt32 pfxCheckParamOfSetupJointConstraints(const PfxSetupJointConstraintsParam &param);
+extern PfxInt32 pfxCheckParamOfSolveConstraints(const PfxSolveConstraintsParam &param);
+
+//----------------------------------------------------------------------------
+//  pfxCheckParamOfSolveConstraints
+//
+/// This function verifies that the input parameter for solving constraints
+/// in parallel is good.
+//----------------------------------------------------------------------------
+PfxInt32 pfxCheckParamOfSolveConstraints(const PfxSolveConstraintsParam &param,
+	PfxTaskManager *taskManager)
+{
+	if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) || 
+		(param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates || !param.offsetSolverBodies) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || 
+		!SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
+		!SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfSolveConstraints(param.numRigidBodies,param.numContactPairs,param.numJointPairs, taskManager->getNumTasks()) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// MULTIPLE THREADS
+
+//----------------------------------------------------------------------------
+//  pfxSetupSolverBodiesTaskEntry
+//
+/// The thread PfxTaskEntry function used to setup solver bodies in parallel.
+//----------------------------------------------------------------------------
+void pfxSetupSolverBodiesTaskEntry(PfxTaskArg *arg)
+{
+	PfxSetupSolverBodiesParam &param = *((PfxSetupSolverBodiesParam*)arg->io);
+
+	PfxUInt32 iFirstBody = arg->data[0];
+	PfxUInt32 iEndBody = arg->data[1];
+
+	PfxRigidState *states = param.states;
+	PfxRigidBody *bodies = param.bodies;
+	PfxSolverBody *solverBodies = param.solverBodies;
+	
+	for(PfxUInt32 i = iFirstBody; i < iEndBody; i++)
+	{
+		PfxRigidState &state = states[i];
+		PfxRigidBody &body = bodies[i];
+		PfxSolverBody &solverBody = solverBodies[i];
+		
+		solverBody.m_orientation = state.getOrientation();
+		solverBody.m_deltaLinearVelocity = PfxVector3(0.0f);
+		solverBody.m_deltaAngularVelocity = PfxVector3(0.0f);
+		solverBody.m_motionType = state.getMotionMask();
+
+		if(SCE_PFX_MOTION_MASK_DYNAMIC(state.getMotionType())) {
+			PfxMatrix3 ori(solverBody.m_orientation);
+			solverBody.m_massInv = body.getMassInv();
+			solverBody.m_inertiaInv = ori * body.getInertiaInv() * transpose(ori);
+		}
+		else {
+			solverBody.m_massInv = 0.0f;
+			solverBody.m_inertiaInv = PfxMatrix3(0.0f);
+		}
+	}
+
+	SCE_PFX_POP_MARKER();
+}
+
+//----------------------------------------------------------------------------
+//  pfxSetupSolverBodies
+//
+/// Perform setup solver bodies in parallel using a task manager.
+///
+/// @param param        Information about rigid bodies
+/// @param taskManager  Pointer to the thread task manager to use
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+PfxInt32 pfxSetupSolverBodies(PfxSetupSolverBodiesParam &param,
+	PfxTaskManager *taskManager)
+{
+	PfxInt32 ret = pfxCheckParamOfSetupSolverBodies(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	SCE_PFX_PUSH_MARKER("pfxSetupSolverBodies");
+
+	PfxUInt32 maxBatchSize = param.numRigidBodies / (PfxUInt32)(taskManager->getNumTasks());
+	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
+	int task = 0;
+	taskManager->setTaskEntry((void*)pfxSetupSolverBodiesTaskEntry);
+
+	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+	}
+
+	// send final task
+	iEnd = param.numRigidBodies;
+	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	SCE_PFX_POP_MARKER();
+	
+	return SCE_PFX_OK;
+}
+
+//----------------------------------------------------------------------------
+//  pfxSetupContactConstraintsTaskEntry
+//
+/// The thread PfxTaskEntry function used to setup contact constraints in
+/// parallel.
+//----------------------------------------------------------------------------
+void pfxSetupContactConstraintsTaskEntry(PfxTaskArg *arg)
+{
+	PfxSetupContactConstraintsParam &param = *((PfxSetupContactConstraintsParam*)arg->io);
+
+	PfxUInt32 iFirstContactPair = arg->data[0];
+	PfxUInt32 iEndContactPair = arg->data[1];
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxUInt32 numContactPairs = param.numContactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
+	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
+	
+	for(PfxUInt32 i = iFirstContactPair; i < iEndContactPair; i++)
+	{
+		PfxConstraintPair &pair = contactPairs[i];
+		if(!pfxCheckSolver(pair))
+		{
+			continue;
+		}
+
+		PfxUInt16 iA = pfxGetObjectIdA(pair);
+		PfxUInt16 iB = pfxGetObjectIdB(pair);
+		PfxUInt32 iConstraint = pfxGetConstraintId(pair);
+
+		PfxContactManifold &contact = offsetContactManifolds[iConstraint];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
+		SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
+
+		PfxRigidState &stateA = offsetRigidStates[iA];
+		PfxRigidBody &bodyA = offsetRigidBodies[iA];
+		PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+
+		PfxRigidState &stateB = offsetRigidStates[iB];
+		PfxRigidBody &bodyB = offsetRigidBodies[iB];
+		PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+	
+		contact.setInternalFlag(0);
+		
+		PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());
+		if(contact.getDuration() > 1) restitution = 0.0f;
+		
+		PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());
+		
+		for(int j=0;j<contact.getNumContacts();j++)
+		{
+			PfxContactPoint &cp = contact.getContactPoint(j);
+			
+			pfxSetupContactConstraint(
+				cp.m_constraintRow[0],
+				cp.m_constraintRow[1],
+				cp.m_constraintRow[2],
+				cp.m_distance,
+				restitution,
+				friction,
+				pfxReadVector3(cp.m_constraintRow[0].m_normal),
+				pfxReadVector3(cp.m_localPointA),
+				pfxReadVector3(cp.m_localPointB),
+				stateA,
+				stateB,
+				solverBodyA,
+				solverBodyB,
+				param.separateBias,
+				param.timeStep
+				);
+		}
+
+		contact.setCompositeFriction(friction);
+	}
+}
+
+//----------------------------------------------------------------------------
+//  pfxSetupContactConstraints
+//
+/// Perform setup contact constraints in parallel using a task manager.
+///
+/// @param param        Information about contact constraints
+/// @param taskManager  Pointer to the thread task manager to use
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+PfxInt32 pfxSetupContactConstraints(PfxSetupContactConstraintsParam &param,
+	PfxTaskManager *taskManager)
+{
+	PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");
+
+	PfxUInt32 maxBatchSize = param.numContactPairs / (PfxUInt32)(taskManager->getNumTasks());
+	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
+	int task = 0;
+	taskManager->setTaskEntry((void*)pfxSetupContactConstraintsTaskEntry);
+
+	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+	}
+
+	// send final task
+	iEnd = param.numContactPairs;
+	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	SCE_PFX_POP_MARKER();
+	
+	return SCE_PFX_OK;
+}
+
+//----------------------------------------------------------------------------
+//  pfxSetupJointConstraintsTaskEntry
+//
+/// The thread PfxTaskEntry function used to setup joint constraints in
+/// parallel.
+//----------------------------------------------------------------------------
+void pfxSetupJointConstraintsTaskEntry(PfxTaskArg *arg)
+{
+	PfxSetupJointConstraintsParam &param = *((PfxSetupJointConstraintsParam*)arg->io);
+
+	PfxUInt32 iFirstJointPair = arg->data[0];
+	PfxUInt32 iEndJointPair = arg->data[1];
+
+	PfxConstraintPair *jointPairs = param.jointPairs;
+	PfxUInt32 numJointPairs = param.numJointPairs;
+	PfxJoint *offsetJoints = param.offsetJoints;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
+	
+	for(PfxUInt32 i = iFirstJointPair; i < iEndJointPair; i++)
+	{
+		PfxConstraintPair &pair = jointPairs[i];
+		if(!pfxCheckSolver(pair))
+		{
+			continue;
+		}
+
+		PfxUInt16 iA = pfxGetObjectIdA(pair);
+		PfxUInt16 iB = pfxGetObjectIdB(pair);
+		PfxUInt32 iConstraint = pfxGetConstraintId(pair);
+		
+		PfxJoint &joint = offsetJoints[iConstraint];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==joint.m_rigidBodyIdA);
+		SCE_PFX_ALWAYS_ASSERT(iB==joint.m_rigidBodyIdB);
+
+		PfxRigidState &stateA = offsetRigidStates[iA];
+		PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+
+		PfxRigidState &stateB = offsetRigidStates[iB];
+		PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+		
+		pfxGetSetupJointConstraintFunc(joint.m_type)(
+			joint,
+			stateA,
+			stateB,
+			solverBodyA,
+			solverBodyB,
+			param.timeStep);
+	}
+}
+
+//----------------------------------------------------------------------------
+//  pfxSetupJointConstraints
+//
+/// Perform setup joint constraints in parallel using a task manager.
+///
+/// @param param        Information about joint constraints
+/// @param taskManager  Pointer to the thread task manager to use
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+PfxInt32 pfxSetupJointConstraints(PfxSetupJointConstraintsParam &param,
+	PfxTaskManager *taskManager)
+{
+	PfxInt32 ret = pfxCheckParamOfSetupJointConstraints(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	SCE_PFX_PUSH_MARKER("pfxSetupJointConstraints");
+
+	PfxUInt32 maxBatchSize = param.numJointPairs / (PfxUInt32)(taskManager->getNumTasks());
+	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
+	int task = 0;
+	taskManager->setTaskEntry((void*)pfxSetupJointConstraintsTaskEntry);
+
+	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+	}
+
+	// send final task
+	iEnd = param.numJointPairs;
+	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	SCE_PFX_POP_MARKER();
+	
+	return SCE_PFX_OK;
+}
+
+//----------------------------------------------------------------------------
+//  pfxSplitConstraints
+//
+/// Given a set of constraints to be solved, split the constraints into
+/// a collection of phases, with each phase having one or more independent
+/// batches that can be solved in parallel. The phases must be solved
+/// sequentially.
+///
+/// @param numRigidBodies    [in] Total number of rigid bodies referenced
+///                          in the given set of constraints
+/// @param constraintpairs   [in] Pointer to array of constraints to split.
+///                          For clarity, note that the pairs always stay
+///                          together. Some pairs are split to be solved
+///                          in parallel with other pairs.
+/// @param numConstraints    [in] Number of constraints to split
+/// @param taskManager       [in] Pointer to the thread task manager that will
+///                          eventually be used to solve the constraints.
+/// @param group             [out] On return, contains information about
+///                          the phases and batches that define the splitting
+/// @param batches           [out] Caller should pass a pointer to a pre-
+///                          allocated array of SCE_PFX_MAX_SOLVER_BATCHES
+///                          PfxParallelBatch objects. On output, these will
+///                          be populated with the correct number of pairs
+///                          for each batch.
+
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+void pfxSplitConstraints(PfxUInt32 numRigidBodies, PfxConstraintPair *constraintPairs,
+	PfxUInt32 numConstraints, PfxTaskManager *taskManager, PfxParallelGroup *group,
+	PfxParallelBatch *batches)
+{
+	SCE_PFX_PUSH_MARKER("pfxSplitConstraints");
+
+	// allocate a table that will be used to indicate, for a given phase being
+	// populated, which batch a given body belongs to.
+	PfxInt32 bufSize = sizeof(PfxUInt8) * numRigidBodies;
+	bufSize = ((bufSize + 127) >> 7) << 7; // 128 bytes alignment
+	PfxUInt8 *bodyTable = (PfxUInt8*)taskManager->allocate(bufSize);
+
+	// allocate a table that will be used to indicate, for a given phase being
+	// populated, which batch a given pair of bodies belongs to.
+	PfxUInt32 *pairTable;
+	size_t allocSize = sizeof(PfxUInt32)*((numConstraints + 31) / 32);
+	pairTable = (PfxUInt32*)taskManager->allocate(allocSize);
+	memset(pairTable, 0, allocSize);
+
+	// 
+	PfxUInt32 numTasks = taskManager->getNumTasks();
+	PfxUInt32 targetCount = SCE_PFX_MAX(PfxUInt32(SCE_PFX_MIN_SOLVER_PAIRS),
+										SCE_PFX_MIN(numConstraints / (numTasks * 2), PfxUInt32(SCE_PFX_MAX_SOLVER_PAIRS)));
+	PfxUInt32 startIndex = 0;
+	
+	PfxUInt32 phaseId;
+	PfxUInt32 batchId;
+	PfxUInt32 totalCount = 0;
+	
+	PfxUInt32 maxBatches = SCE_PFX_MIN(numTasks, PfxUInt32(SCE_PFX_MAX_SOLVER_BATCHES));
+
+	// accumulate phases and batches until group resources are exhausted or all incoming
+	// pairs are accounted for.
+	for (phaseId = 0; phaseId < SCE_PFX_MAX_SOLVER_PHASES && totalCount < numConstraints; phaseId++)
+	{
+		bool startIndexCheck = true;
+
+		group->numBatches[phaseId] = 0;
+
+		PfxUInt32 i = startIndex;
+		// Initialize body table such that no body is assigned to any batch. (0xff is explicitly assumed to
+		// mean no batch assigned)
+		memset(bodyTable, 0xff, bufSize);
+
+		// accumulate batches within the current phase. This code creates batches that are
+		// independent, e.g., no batch on this phase will touch the same bodies as any other
+		// batch on the phase. Batches within a phase can be solved in parallel on shared memory
+		// multiprocessor hardware.
+		for (batchId = 0; i < numConstraints && totalCount < numConstraints && batchId < maxBatches; batchId++)
+		{
+			PfxUInt32 pairCount=0;
+			PfxParallelBatch &batch = batches[(phaseId * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+			PfxUInt32 pairId = 0;
+
+			// iterate through pairs, and assigns the pairs to batches
+			for (; i < numConstraints && pairCount < targetCount; i++)
+			{
+				PfxUInt32 idxP = i >> 5;
+				PfxUInt32 maskP = 1L << (i & 31);
+
+				if(pairTable[idxP] & maskP)	// pair is already assigned to a phase/batch
+					continue;
+
+				PfxUInt16 idxA = pfxGetObjectIdA(constraintPairs[i]);
+				PfxUInt16 idxB = pfxGetObjectIdB(constraintPairs[i]);
+
+				// It is possible an incoming constraint pair can be skipped. For example, if the pair is inactive,
+				// or if both its objects are static, unmoving objects and therefore would be unaffected by constraints.
+				// This conditional statement addresses constraints to be skipped.
+				if (!pfxGetActive(constraintPairs[i]) ||
+					(SCE_PFX_MOTION_MASK_STATIC(pfxGetMotionMaskA(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE) &&
+					 SCE_PFX_MOTION_MASK_STATIC(pfxGetMotionMaskB(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE)) )
+				{
+					if (startIndexCheck) 
+						startIndex++;
+
+					//assign pair -> skip it because it has no constraints
+					pairTable[idxP] |= maskP;
+					totalCount++;
+					continue;
+				}
+
+				// If either body of the current pair belongs to another batch already, we cannot add the current
+				// pair to the current batch. Must defer to another phase
+				if( (bodyTable[idxA] != batchId && bodyTable[idxA] != 0xff) || 
+					(bodyTable[idxB] != batchId && bodyTable[idxB] != 0xff) )
+				{
+					startIndexCheck = false; // so we will revisit this during allocation of next phase
+					continue;
+				}
+
+				// Dynamic bodies for current pair are assigned to the current batch in this phase.
+				// Static bodies are not assigned. Since they never move, and their corresponding solver
+				// bodies are therefore never touched, they can actually be used by any batch.
+				if (SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskA(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE))
+					bodyTable[idxA] = batchId;
+
+				if (SCE_PFX_MOTION_MASK_DYNAMIC(pfxGetMotionMaskB(constraintPairs[i])&SCE_PFX_MOTION_MASK_TYPE))
+					bodyTable[idxB] = batchId;
+
+				if(startIndexCheck)
+					startIndex++;
+
+				pairTable[idxP] |= maskP;	// pair has been handled
+
+				//add the pair 'i' to the current batch
+				batch.pairIndices[pairId++] = i;
+				pairCount++;
+			}
+
+			group->numPairs[(phaseId * SCE_PFX_MAX_SOLVER_BATCHES) + batchId] = (PfxUInt16)pairId;
+			totalCount += pairCount;
+		}
+		group->numBatches[phaseId] = batchId;
+	}
+
+	group->numPhases = phaseId;
+
+	taskManager->deallocate(bodyTable);
+	taskManager->deallocate(pairTable);
+
+	SCE_PFX_POP_MARKER();
+}
+
+//----------------------------------------------------------------------------
+//  pfxSolveConstraintsTaskEntry
+//
+/// The thread PfxTaskEntry function used to solve constraints in parallel.
+//----------------------------------------------------------------------------
+void pfxSolveConstraintsTaskEntry(PfxTaskArg *arg)
+{
+	PfxSolveConstraintsParam &param = *((PfxSolveConstraintsParam*)arg->io);
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxConstraintPair *jointPairs = param.jointPairs;
+	PfxJoint *offsetJoints = param.offsetJoints;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
+
+	PfxParallelGroup *jointgroup = (PfxParallelGroup*)arg->data[0];
+	PfxParallelBatch *jointbatches = (PfxParallelBatch*)arg->data[1];
+	PfxParallelGroup *contactgroup = (PfxParallelGroup*)arg->data[2];
+	PfxParallelBatch *contactbatches = (PfxParallelBatch*)arg->data[3];
+
+	// Warm Starting
+	{
+		// Joints
+		for (PfxUInt16 phase = 0; phase < jointgroup->numPhases; phase++)
+		{
+			for (PfxUInt16 batchId = 0; batchId < jointgroup->numBatches[phase]; batchId++)
+			{
+				PfxUInt16 numJointPairs = jointgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+				if ((arg->taskId == (batchId % arg->maxTasks)) && numJointPairs > 0) // only spend time on batches meant for this task
+				{
+					const PfxParallelBatch &batch = jointbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+					for (PfxUInt16 i = 0; i < numJointPairs; i++)
+					{
+						PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];
+						if(!pfxCheckSolver(pair))
+							continue;
+
+						PfxUInt16 iA = pfxGetObjectIdA(pair);
+						PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+						PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
+
+						SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
+						SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
+
+						PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+						PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+						pfxGetWarmStartJointConstraintFunc(joint.m_type)(
+							joint,
+							solverBodyA,
+							solverBodyB);
+					}
+				}
+
+				arg->barrier->sync();	// block until all threads are ready to go to next phase
+			}
+		}
+
+		// Contacts
+		for (PfxUInt16 phase = 0; phase < contactgroup->numPhases; phase++)
+		{
+			for (PfxUInt16 batchId = 0; batchId < contactgroup->numBatches[phase]; batchId++)
+			{
+				PfxUInt16 numContactPairs = contactgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+				if ((arg->taskId == (batchId % arg->maxTasks)) && numContactPairs > 0) // only spend time on batches meant for this task
+				{
+					const PfxParallelBatch &batch = contactbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+					for (PfxUInt16 i = 0; i < numContactPairs; i++)
+					{
+						PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];
+						if(!pfxCheckSolver(pair))
+						{
+							continue;
+						}
+
+						PfxUInt16 iA = pfxGetObjectIdA(pair);
+						PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+						PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
+
+						SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
+						SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
+
+						PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+						PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+						PfxFloat massInvA = solverBodyA.m_massInv;
+						PfxFloat massInvB = solverBodyB.m_massInv;
+						PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
+						PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
+
+						if(solverBodyA.m_motionType == kPfxMotionTypeOneWay)
+						{
+							massInvB = 0.0f;
+							inertiaInvB = PfxMatrix3(0.0f);
+						}
+
+						if(solverBodyB.m_motionType == kPfxMotionTypeOneWay)
+						{
+							massInvA = 0.0f;
+							inertiaInvA = PfxMatrix3(0.0f);
+						}
+
+						for(int j = 0; j < contact.getNumContacts(); j++)
+						{
+							PfxContactPoint &cp = contact.getContactPoint(j);
+				
+							PfxVector3 rA = rotate(solverBodyA.m_orientation,pfxReadVector3(cp.m_localPointA));
+							PfxVector3 rB = rotate(solverBodyB.m_orientation,pfxReadVector3(cp.m_localPointB));
+				
+							for(int k = 0; k < 3; k++)
+							{
+								PfxVector3 normal = pfxReadVector3(cp.m_constraintRow[k].m_normal);
+								PfxFloat deltaImpulse = cp.m_constraintRow[k].m_accumImpulse;
+								solverBodyA.m_deltaLinearVelocity += deltaImpulse * massInvA * normal;
+								solverBodyA.m_deltaAngularVelocity += deltaImpulse * inertiaInvA * cross(rA,normal);
+								solverBodyB.m_deltaLinearVelocity -= deltaImpulse * massInvB * normal;
+								solverBodyB.m_deltaAngularVelocity -= deltaImpulse * inertiaInvB * cross(rB,normal);
+							}
+						}
+					}
+				}
+			}
+
+			arg->barrier->sync();	// block until all threads are ready to go to next phase
+		}
+	}
+	
+	// Solver
+	for(PfxUInt32 iteration = 0; iteration < param.iteration; iteration++)
+	{
+		// Joints
+		for (PfxUInt16 phase = 0; phase < jointgroup->numPhases; phase++)
+		{
+			for (PfxUInt16 batchId = 0; batchId < jointgroup->numBatches[phase]; batchId++)
+			{
+				PfxUInt16 numJointPairs = jointgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+				if ((arg->taskId == (batchId % arg->maxTasks)) && numJointPairs > 0) // only spend time on batches meant for this task
+				{
+					const PfxParallelBatch &batch = jointbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+					for(PfxUInt16 i = 0; i < numJointPairs; i++)
+					{
+						PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];
+						if(!pfxCheckSolver(pair))
+							continue;
+
+						PfxUInt16 iA = pfxGetObjectIdA(pair);
+						PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+						PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
+
+						SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
+						SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
+
+						PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+						PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+						pfxGetSolveJointConstraintFunc(joint.m_type)(
+							joint,
+							solverBodyA,
+							solverBodyB);
+					}
+				}
+			}
+
+			arg->barrier->sync();	// block until all threads are ready to go to next phase
+		}
+
+		// Contacts
+		for (PfxUInt32 phase = 0; phase < contactgroup->numPhases; phase++)
+		{
+			for (PfxUInt32 batchId = 0; batchId < contactgroup->numBatches[phase]; batchId++)
+			{
+				PfxUInt32 numContactPairs = contactgroup->numPairs[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+				if ((arg->taskId == (batchId % arg->maxTasks)) && numContactPairs > 0) // only spend time on batches meant for this task
+				{
+					const PfxParallelBatch &batch = contactbatches[(phase * SCE_PFX_MAX_SOLVER_BATCHES) + batchId];
+					for(PfxUInt32 i = 0; i < numContactPairs; i++)
+					{
+						PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];
+						if(!pfxCheckSolver(pair))
+							continue;
+
+						PfxUInt16 iA = pfxGetObjectIdA(pair);
+						PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+						PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
+
+						SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
+						SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
+
+						PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+						PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+						for(int j = 0; j < contact.getNumContacts(); j++)
+						{
+							PfxContactPoint &cp = contact.getContactPoint(j);
+				
+							pfxSolveContactConstraint(
+								cp.m_constraintRow[0],
+								cp.m_constraintRow[1],
+								cp.m_constraintRow[2],
+								pfxReadVector3(cp.m_localPointA),
+								pfxReadVector3(cp.m_localPointB),
+								solverBodyA,
+								solverBodyB,
+								contact.getCompositeFriction()
+								);
+						}
+					}
+				}
+			}
+
+			arg->barrier->sync();	// block until all threads are ready to go to next phase
+		}
+	}
+}
+
+//----------------------------------------------------------------------------
+//  pfxSolveConstraints
+//
+/// Perform setup joint constraints in parallel using a task manager.
+///
+/// @param param        Information about constraints
+/// @param taskManager  Pointer to the thread task manager to use
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+PfxInt32 pfxSolveConstraints(PfxSolveConstraintsParam &param,
+	PfxTaskManager *taskManager)
+{
+	PfxInt32 ret = pfxCheckParamOfSolveConstraints(param);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxSolveConstraints");
+
+	PfxParallelGroup *contactgroup = (PfxParallelGroup*)taskManager->allocate(sizeof(PfxParallelGroup));
+	PfxParallelBatch *contactbatches = (PfxParallelBatch*)taskManager->allocate(sizeof(PfxParallelBatch) * (SCE_PFX_MAX_SOLVER_PHASES * SCE_PFX_MAX_SOLVER_BATCHES));
+	PfxParallelGroup *jointgroup = (PfxParallelGroup*)taskManager->allocate(sizeof(PfxParallelGroup));
+	PfxParallelBatch *jointbatches = (PfxParallelBatch*)taskManager->allocate(sizeof(PfxParallelBatch) * (SCE_PFX_MAX_SOLVER_PHASES * SCE_PFX_MAX_SOLVER_BATCHES));
+
+	// split constraints into independent phases and batches. Phases allow
+	// a set of non-independent constraints to be solved in parallel. One
+	// phase may have dependencies within another phase, but the phases are
+	// solved sequentially. Within a phases, there are multiple batches, and
+	// the batches are independent. Since they are independent, they can be
+	// distributed to different processors and solved in parallel.
+	pfxSplitConstraints(param.numRigidBodies, param.contactPairs, param.numContactPairs,
+						taskManager, contactgroup, contactbatches);
+	pfxSplitConstraints(param.numRigidBodies, param.jointPairs, param.numJointPairs,
+						taskManager, jointgroup, jointbatches);
+
+	// parallel solve
+	taskManager->setTaskEntry((void*)pfxSolveConstraintsTaskEntry);
+	int task = 0;
+	for (; task < taskManager->getNumTasks(); task++)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), (PfxUInt32)jointgroup, (PfxUInt32)jointbatches,
+								(PfxUInt32)contactgroup, (PfxUInt32)contactbatches);
+	}
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	// post solve
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
+	for (PfxUInt32 i = 0; i < param.numRigidBodies; i++)
+	{
+		param.offsetRigidStates[i].setLinearVelocity(param.offsetRigidStates[i].getLinearVelocity() +
+														param.offsetSolverBodies[i].m_deltaLinearVelocity);
+		param.offsetRigidStates[i].setAngularVelocity(param.offsetRigidStates[i].getAngularVelocity() +
+														param.offsetSolverBodies[i].m_deltaAngularVelocity);
+	}
+
+	taskManager->clearPool();
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/solver/pfx_constraint_solver_single.cpp

@@ -0,0 +1,399 @@
+/*
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_contact_constraint.h"
+#include "../../../include/physics_effects/low_level/solver/pfx_joint_constraint_func.h"
+#include "../../../include/physics_effects/low_level/solver/pfx_constraint_solver.h"
+#include "../../base_level/solver/pfx_check_solver.h"
+#include "pfx_parallel_group.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxUInt32 pfxGetWorkBytesOfSolveConstraints(PfxUInt32 numRigidBodies,PfxUInt32 numContactPairs,PfxUInt32 numJointPairs,PfxUInt32 maxTasks)
+{
+	(void)maxTasks;
+	PfxUInt32 workBytes = SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxUInt8) * numRigidBodies) +
+		SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxUInt32)*((SCE_PFX_MAX(numContactPairs,numJointPairs)+31)/32));
+
+	workBytes += 128 + (SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxParallelGroup)) + 
+		 SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxParallelBatch)*(SCE_PFX_MAX_SOLVER_PHASES*SCE_PFX_MAX_SOLVER_BATCHES))) * 2;
+
+	return workBytes;
+}
+
+PfxInt32 pfxCheckParamOfSetupSolverBodies(const PfxSetupSolverBodiesParam &param)
+{
+	if(!param.states || !param.bodies || !param.solverBodies ) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.states) || !SCE_PFX_PTR_IS_ALIGNED16(param.bodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.solverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxCheckParamOfSetupContactConstraints(const PfxSetupContactConstraintsParam &param)
+{
+	if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) || !param.offsetRigidStates || 
+		!param.offsetRigidBodies  || !param.offsetSolverBodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
+		!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidBodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxCheckParamOfSetupJointConstraints(const PfxSetupJointConstraintsParam &param)
+{
+	if((param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates || 
+		!param.offsetRigidBodies  || !param.offsetSolverBodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
+		!SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidBodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxCheckParamOfSolveConstraints(const PfxSolveConstraintsParam &param)
+{
+	if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) || 
+		(param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates || !param.offsetSolverBodies) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || 
+		!SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||
+		!SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfSolveConstraints(param.numRigidBodies,param.numContactPairs,param.numJointPairs) ) return SCE_PFX_ERR_OUT_OF_BUFFER;
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// SINGLE THREAD
+
+PfxInt32 pfxSetupSolverBodies(PfxSetupSolverBodiesParam &param)
+{
+	PfxInt32 ret = pfxCheckParamOfSetupSolverBodies(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	SCE_PFX_PUSH_MARKER("pfxSetupSolverBodies");
+
+	PfxRigidState *states = param.states;
+	PfxRigidBody *bodies = param.bodies;
+	PfxSolverBody *solverBodies = param.solverBodies;
+	PfxUInt32 numRigidBodies = param.numRigidBodies;
+	
+	for(PfxUInt32 i=0;i<numRigidBodies;i++) {
+		PfxRigidState &state = states[i];
+		PfxRigidBody &body = bodies[i];
+		PfxSolverBody &solverBody = solverBodies[i];
+		
+		solverBody.m_orientation = state.getOrientation();
+		solverBody.m_deltaLinearVelocity = PfxVector3(0.0f);
+		solverBody.m_deltaAngularVelocity = PfxVector3(0.0f);
+		solverBody.m_motionType = state.getMotionMask();
+
+		if(SCE_PFX_MOTION_MASK_DYNAMIC(state.getMotionType())) {
+			PfxMatrix3 ori(solverBody.m_orientation);
+			solverBody.m_massInv = body.getMassInv();
+			solverBody.m_inertiaInv = ori * body.getInertiaInv() * transpose(ori);
+		}
+		else {
+			solverBody.m_massInv = 0.0f;
+			solverBody.m_inertiaInv = PfxMatrix3(0.0f);
+		}
+	}
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxSetupContactConstraints(PfxSetupContactConstraintsParam &param)
+{
+	PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxUInt32 numContactPairs = param.numContactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
+	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
+	
+	for(PfxUInt32 i=0;i<numContactPairs;i++) {
+		PfxConstraintPair &pair = contactPairs[i];
+		if(!pfxCheckSolver(pair)) {
+			continue;
+		}
+
+		PfxUInt16 iA = pfxGetObjectIdA(pair);
+		PfxUInt16 iB = pfxGetObjectIdB(pair);
+		PfxUInt32 iConstraint = pfxGetConstraintId(pair);
+
+		PfxContactManifold &contact = offsetContactManifolds[iConstraint];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
+		SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
+
+		PfxRigidState &stateA = offsetRigidStates[iA];
+		PfxRigidBody &bodyA = offsetRigidBodies[iA];
+		PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+
+		PfxRigidState &stateB = offsetRigidStates[iB];
+		PfxRigidBody &bodyB = offsetRigidBodies[iB];
+		PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+	
+		contact.setInternalFlag(0);
+		
+		PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());
+		if(contact.getDuration() > 1) restitution = 0.0f;
+		
+		PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());
+		
+		for(int j=0;j<contact.getNumContacts();j++) {
+			PfxContactPoint &cp = contact.getContactPoint(j);
+			
+			pfxSetupContactConstraint(
+				cp.m_constraintRow[0],
+				cp.m_constraintRow[1],
+				cp.m_constraintRow[2],
+				cp.m_distance,
+				restitution,
+				friction,
+				pfxReadVector3(cp.m_constraintRow[0].m_normal),
+				pfxReadVector3(cp.m_localPointA),
+				pfxReadVector3(cp.m_localPointB),
+				stateA,
+				stateB,
+				solverBodyA,
+				solverBodyB,
+				param.separateBias,
+				param.timeStep
+				);
+		}
+
+		contact.setCompositeFriction(friction);
+	}
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxSetupJointConstraints(PfxSetupJointConstraintsParam &param)
+{
+	PfxInt32 ret = pfxCheckParamOfSetupJointConstraints(param);
+	if(ret != SCE_PFX_OK) return ret;
+	
+	SCE_PFX_PUSH_MARKER("pfxSetupJointConstraints");
+
+	PfxConstraintPair *jointPairs = param.jointPairs;
+	PfxUInt32 numJointPairs = param.numJointPairs;
+	PfxJoint *offsetJoints = param.offsetJoints;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
+	
+	for(PfxUInt32 i=0;i<numJointPairs;i++) {
+		PfxConstraintPair &pair = jointPairs[i];
+		if(!pfxCheckSolver(pair)) {
+			continue;
+		}
+
+		PfxUInt16 iA = pfxGetObjectIdA(pair);
+		PfxUInt16 iB = pfxGetObjectIdB(pair);
+		PfxUInt32 iConstraint = pfxGetConstraintId(pair);
+		
+		PfxJoint &joint = offsetJoints[iConstraint];
+
+		SCE_PFX_ALWAYS_ASSERT(iA==joint.m_rigidBodyIdA);
+		SCE_PFX_ALWAYS_ASSERT(iB==joint.m_rigidBodyIdB);
+
+		PfxRigidState &stateA = offsetRigidStates[iA];
+		PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+
+		PfxRigidState &stateB = offsetRigidStates[iB];
+		PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+		
+		pfxGetSetupJointConstraintFunc(joint.m_type)(
+			joint,
+			stateA,
+			stateB,
+			solverBodyA,
+			solverBodyB,
+			param.timeStep);
+	}
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxSolveConstraints(PfxSolveConstraintsParam &param)
+{
+	PfxInt32 ret = pfxCheckParamOfSolveConstraints(param);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxSolveConstraints");
+
+	PfxConstraintPair *contactPairs = param.contactPairs;
+	PfxUInt32 numContactPairs = param.numContactPairs;
+	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
+	PfxConstraintPair *jointPairs = param.jointPairs;
+	PfxUInt32 numJointPairs = param.numJointPairs;
+	PfxJoint *offsetJoints = param.offsetJoints;
+	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
+	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
+	PfxUInt32 numRigidBodies = param.numRigidBodies;
+	
+	// Warm Starting
+	{
+		for(PfxUInt32 i=0;i<numJointPairs;i++) {
+			PfxConstraintPair &pair = jointPairs[i];
+			if(!pfxCheckSolver(pair)) {
+				continue;
+			}
+
+			PfxUInt16 iA = pfxGetObjectIdA(pair);
+			PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+			PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
+
+			SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
+			SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
+
+			PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+			PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+			pfxGetWarmStartJointConstraintFunc(joint.m_type)(
+				joint,
+				solverBodyA,
+				solverBodyB);
+		}
+		for(PfxUInt32 i=0;i<numContactPairs;i++) {
+			PfxConstraintPair &pair = contactPairs[i];
+			if(!pfxCheckSolver(pair)) {
+				continue;
+			}
+
+			PfxUInt16 iA = pfxGetObjectIdA(pair);
+			PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+			PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
+
+			SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
+			SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
+
+			PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+			PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+			PfxFloat massInvA = solverBodyA.m_massInv;
+			PfxFloat massInvB = solverBodyB.m_massInv;
+			PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
+			PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
+
+			if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {
+				massInvB = 0.0f;
+				inertiaInvB = PfxMatrix3(0.0f);
+			}
+			if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {
+				massInvA = 0.0f;
+				inertiaInvA = PfxMatrix3(0.0f);
+			}
+
+			for(int j=0;j<contact.getNumContacts();j++) {
+				PfxContactPoint &cp = contact.getContactPoint(j);
+				
+				PfxVector3 rA = rotate(solverBodyA.m_orientation,pfxReadVector3(cp.m_localPointA));
+				PfxVector3 rB = rotate(solverBodyB.m_orientation,pfxReadVector3(cp.m_localPointB));
+				
+				for(int k=0;k<3;k++) {
+					PfxVector3 normal = pfxReadVector3(cp.m_constraintRow[k].m_normal);
+					PfxFloat deltaImpulse = cp.m_constraintRow[k].m_accumImpulse;
+					solverBodyA.m_deltaLinearVelocity += deltaImpulse * massInvA * normal;
+					solverBodyA.m_deltaAngularVelocity += deltaImpulse * inertiaInvA * cross(rA,normal);
+					solverBodyB.m_deltaLinearVelocity -= deltaImpulse * massInvB * normal;
+					solverBodyB.m_deltaAngularVelocity -= deltaImpulse * inertiaInvB * cross(rB,normal);
+				}
+			}
+		}
+	}
+	
+	// Solver
+	for(PfxUInt32 iteration=0;iteration<param.iteration;iteration++) {
+		for(PfxUInt32 i=0;i<numJointPairs;i++) {
+			PfxConstraintPair &pair = jointPairs[i];
+			if(!pfxCheckSolver(pair)) {
+				continue;
+			}
+
+			PfxUInt16 iA = pfxGetObjectIdA(pair);
+			PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+			PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];
+
+			SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);
+			SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);
+
+			PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+			PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+			pfxGetSolveJointConstraintFunc(joint.m_type)(
+				joint,
+				solverBodyA,
+				solverBodyB);
+		}
+		for(PfxUInt32 i=0;i<numContactPairs;i++) {
+			PfxConstraintPair &pair = contactPairs[i];
+			if(!pfxCheckSolver(pair)) {
+				continue;
+			}
+
+			PfxUInt16 iA = pfxGetObjectIdA(pair);
+			PfxUInt16 iB = pfxGetObjectIdB(pair);
+
+			PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];
+
+			SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());
+			SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());
+
+			PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
+			PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
+			
+			for(int j=0;j<contact.getNumContacts();j++) {
+				PfxContactPoint &cp = contact.getContactPoint(j);
+				
+				pfxSolveContactConstraint(
+					cp.m_constraintRow[0],
+					cp.m_constraintRow[1],
+					cp.m_constraintRow[2],
+					pfxReadVector3(cp.m_localPointA),
+					pfxReadVector3(cp.m_localPointB),
+					solverBodyA,
+					solverBodyB,
+					contact.getCompositeFriction()
+					);
+			}
+		}
+	}
+
+	for(PfxUInt32 i=0;i<numRigidBodies;i++) {
+		offsetRigidStates[i].setLinearVelocity(
+			offsetRigidStates[i].getLinearVelocity()+offsetSolverBodies[i].m_deltaLinearVelocity);
+		offsetRigidStates[i].setAngularVelocity(
+			offsetRigidStates[i].getAngularVelocity()+offsetSolverBodies[i].m_deltaAngularVelocity);
+	}
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/solver/pfx_joint_constraint_func.cpp

@@ -0,0 +1,180 @@
+/*
+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/solver/pfx_joint.h"
+#include "../../../include/physics_effects/low_level/solver/pfx_joint_constraint_func.h"
+
+#include "../../../include/physics_effects/base_level/solver/pfx_joint_ball.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_joint_swing_twist.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_joint_hinge.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_joint_slider.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_joint_fix.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_joint_universal.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Setup Joint Constraint Function Table
+
+void setupJointConstraintDummy(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep)
+{
+	(void)joint,(void)stateA,(void)stateB,(void)solverBodyA,(void)solverBodyB,(void)timeStep;
+}
+
+PfxSetupJointConstraintFunc funcTbl_setupJointConstraint[kPfxJointCount] = {
+	pfxSetupBallJoint,
+	pfxSetupSwingTwistJoint,
+	pfxSetupSwingTwistJoint,
+	pfxSetupSwingTwistJoint,
+	pfxSetupSwingTwistJoint,
+	pfxSetupUniversalJoint,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+	setupJointConstraintDummy,
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Setup Joint Constraint Function Table Interface
+
+PfxSetupJointConstraintFunc pfxGetSetupJointConstraintFunc(PfxUInt8 jointType)
+{
+	SCE_PFX_ASSERT(jointType<kPfxJointCount);
+	return funcTbl_setupJointConstraint[jointType];
+}
+
+int pfxSetSetupJointConstraintFunc(PfxUInt8 jointType,PfxSetupJointConstraintFunc func)
+{
+	if(jointType >= kPfxJointCount) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	
+	funcTbl_setupJointConstraint[jointType] = func;
+	
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Warm Start Joint Constraint Function Table
+
+void warmStartJointConstraintDummy(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB)
+{
+	(void)joint,(void)solverBodyA,(void)solverBodyB;
+}
+
+PfxWarmStartJointConstraintFunc funcTbl_warmStartJointConstraint[kPfxJointCount] = {
+	pfxWarmStartBallJoint,
+	pfxWarmStartSwingTwistJoint,
+	pfxWarmStartSwingTwistJoint,
+	pfxWarmStartSwingTwistJoint,
+	pfxWarmStartSwingTwistJoint,
+	pfxWarmStartSwingTwistJoint,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+	warmStartJointConstraintDummy,
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Warm Start Joint Constraint Function Table Interface
+
+PfxWarmStartJointConstraintFunc pfxGetWarmStartJointConstraintFunc(PfxUInt8 jointType)
+{
+	SCE_PFX_ASSERT(jointType<kPfxJointCount);
+	return funcTbl_warmStartJointConstraint[jointType];
+}
+
+int pfxSetWarmStartJointConstraintFunc(PfxUInt8 jointType,PfxWarmStartJointConstraintFunc func)
+{
+	if(jointType >= kPfxJointCount) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	
+	funcTbl_warmStartJointConstraint[jointType] = func;
+	
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Solve Joint Constraint Function Table
+
+void solveJointConstraintDummy(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB)
+{
+	(void)joint,(void)solverBodyA,(void)solverBodyB;
+}
+
+PfxSolveJointConstraintFunc funcTbl_solveJointConstraint[kPfxJointCount] = {
+	pfxSolveBallJoint,
+	pfxSolveSwingTwistJoint,
+	pfxSolveSwingTwistJoint,
+	pfxSolveSwingTwistJoint,
+	pfxSolveSwingTwistJoint,
+	pfxSolveSwingTwistJoint,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+	solveJointConstraintDummy,
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Solve Joint Constraint Function Table Interface
+
+PfxSolveJointConstraintFunc pfxGetSolveJointConstraintFunc(PfxUInt8 jointType)
+{
+	SCE_PFX_ASSERT(jointType<kPfxJointCount);
+	return funcTbl_solveJointConstraint[jointType];
+}
+
+int pfxSetSolveJointConstraintFunc(PfxUInt8 jointType,PfxSolveJointConstraintFunc func)
+{
+	if(jointType >= kPfxJointCount) {
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+	
+	funcTbl_solveJointConstraint[jointType] = func;
+	
+	return SCE_PFX_OK;
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/solver/pfx_parallel_group.h

@@ -0,0 +1,44 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_PARALLEL_GROUP_H
+#define _SCE_PFX_PARALLEL_GROUP_H
+
+///////////////////////////////////////////////////////////////////////////////
+// Parallel Group
+
+#define SCE_PFX_MAX_SOLVER_PHASES 64	// 最大フェーズ数
+#define SCE_PFX_MAX_SOLVER_BATCHES 32	// １フェーズに含まれる最大並列処理バッチ
+#define SCE_PFX_MAX_SOLVER_PAIRS  64	// １バッチに含まれる最大ペア数
+#define SCE_PFX_MIN_SOLVER_PAIRS  16	// １バッチに含まれる最小ペア数
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct SCE_PFX_ALIGNED(128) PfxParallelBatch {
+	PfxUInt16 pairIndices[SCE_PFX_MAX_SOLVER_PAIRS];
+};
+
+struct SCE_PFX_ALIGNED(128) PfxParallelGroup {
+	PfxUInt16 numPhases;
+	PfxUInt16 numBatches[SCE_PFX_MAX_SOLVER_PHASES]; // 各フェーズの保持する並列実行可能なバッチの数
+	PfxUInt16 numPairs[SCE_PFX_MAX_SOLVER_PHASES*SCE_PFX_MAX_SOLVER_BATCHES]; // 各バッチの保持するペアの数
+SCE_PFX_PADDING(1,126)
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_PARALLEL_GROUP_H

Extras/PhysicsEffects/src/low_level/solver/pfx_update_rigid_states_parallel.cpp

@@ -0,0 +1,94 @@
+/*
+Applied Research Associates Inc. (c)2011
+
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_integrate.h"
+#include "../../../include/physics_effects/low_level/solver/pfx_update_rigid_states.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// This function is implemented in pfx_update_rigid_states_single.cpp
+extern PfxInt32 pfxCheckParamOfUpdateRigidStates(const PfxUpdateRigidStatesParam &param);
+
+///////////////////////////////////////////////////////////////////////////////
+// MULTIPLE THREADS
+
+//----------------------------------------------------------------------------
+//  pfxUpdateRigidStatesTaskEntry
+//
+/// The thread PfxTaskEntry function used to update rigid body states in
+/// parallel.
+//----------------------------------------------------------------------------
+void pfxUpdateRigidStatesTaskEntry(PfxTaskArg *arg)
+{
+	PfxUpdateRigidStatesParam &param = *((PfxUpdateRigidStatesParam*)arg->io);
+
+	PfxUInt32 iFirstBody = arg->data[0];
+	PfxUInt32 iEndBody = arg->data[1];
+
+	for(PfxUInt32 i = iFirstBody; i < iEndBody; i++)
+	{
+		pfxIntegrate(param.states[i],param.bodies[i],param.timeStep);
+	}
+}
+
+//----------------------------------------------------------------------------
+//  pfxUpdateRigidStates
+//
+/// Perform update rigid states in parallel using a task manager.
+///
+/// @param param        Information about rigid bodies
+/// @param taskManager  Pointer to the thread task manager to use
+///
+/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
+//----------------------------------------------------------------------------
+PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param, PfxTaskManager *taskManager)
+{
+	PfxInt32 ret = pfxCheckParamOfUpdateRigidStates(param);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxUpdateRigidStates");
+
+	PfxUInt32 maxBatchSize = param.numRigidBodies / (PfxUInt32)(taskManager->getNumTasks());
+	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
+	int task = 0;
+	taskManager->setTaskEntry((void*)pfxUpdateRigidStatesTaskEntry);
+
+	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
+	{
+		taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+	}
+
+	// send final task
+	iEnd = param.numRigidBodies;
+	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);
+
+	// wait for tasks to complete
+	PfxUInt32 data1, data2, data3, data4;
+	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
+		taskManager->waitTask(task, data1, data2, data3, data4);
+
+	SCE_PFX_POP_MARKER();
+	
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/solver/pfx_update_rigid_states_single.cpp

@@ -0,0 +1,51 @@
+/*
+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_perf_counter.h"
+#include "../../../include/physics_effects/base_level/solver/pfx_integrate.h"
+#include "../../../include/physics_effects/low_level/solver/pfx_update_rigid_states.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxCheckParamOfUpdateRigidStates(const PfxUpdateRigidStatesParam &param)
+{
+	if(!param.states || !param.bodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;
+	if(!SCE_PFX_PTR_IS_ALIGNED16(param.states) || !SCE_PFX_PTR_IS_ALIGNED16(param.bodies)) return SCE_PFX_ERR_INVALID_ALIGN;
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// SINGLE THREAD
+
+PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param)
+{
+	PfxInt32 ret = pfxCheckParamOfUpdateRigidStates(param);
+	if(ret != SCE_PFX_OK) return ret;
+
+	SCE_PFX_PUSH_MARKER("pfxUpdateRigidStates");
+
+	for(PfxUInt32 i=0;i<param.numRigidBodies;i++) {
+		pfxIntegrate(param.states[i],param.bodies[i],param.timeStep);
+	}
+
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/sort/pfx_parallel_sort_single.cpp

@@ -0,0 +1,56 @@
+/*
+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_perf_counter.h"
+#include "../../../include/physics_effects/low_level/sort/pfx_parallel_sort.h"
+#include "../../../include/physics_effects/base_level/sort/pfx_sort.h"
+
+///////////////////////////////////////////////////////////////////////////////
+// SINGLE THREAD
+
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxParallelSort(
+	PfxSortData16 *data,PfxUInt32 numData,
+	void *workBuff,PfxUInt32 workBytes)
+{
+	if(!SCE_PFX_PTR_IS_ALIGNED16(workBuff)) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(workBuff,workBytes) < sizeof(PfxSortData16) * numData) return SCE_PFX_ERR_OUT_OF_BUFFER;
+
+	SCE_PFX_PUSH_MARKER("pfxParallelSort");
+	pfxSort(data,(PfxSortData16*)workBuff,numData);
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+PfxInt32 pfxParallelSort(
+	PfxSortData32 *data,PfxUInt32 numData,
+	void *workBuff,PfxUInt32 workBytes)
+{
+	if(!SCE_PFX_PTR_IS_ALIGNED16(workBuff)) return SCE_PFX_ERR_INVALID_ALIGN;
+	if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(workBuff,workBytes) < sizeof(PfxSortData32) * numData) return SCE_PFX_ERR_OUT_OF_BUFFER;
+
+	SCE_PFX_PUSH_MARKER("pfxParallelSort");
+	pfxSort(data,(PfxSortData32*)workBuff,numData);
+	SCE_PFX_POP_MARKER();
+
+	return SCE_PFX_OK;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/low_level/task/pfx_sync_components_pthreads.cpp

@@ -0,0 +1,237 @@
+/*
+ Applied Research Associates Inc. (c)2011
+
+ Redistribution and use in source and binary forms,
+   with or without modification, are permitted provided that the
+   following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Applied Research Associates Inc nor the names
+      of its contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifdef __ANDROID__
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+#include "../../../include/physics_effects/low_level/task/pfx_pthreads.h"
+#include "../../../include/physics_effects/low_level/task/pfx_sync_components.h"
+#include "pfx_sync_components_pthreads.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsBarrier::PfxPthreadsBarrier
+//
+/// Default constructor
+//----------------------------------------------------------------------------
+PfxPthreadsBarrier::PfxPthreadsBarrier() :
+	m_maxThreads(0), m_called(0)
+{
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsBarrier::~PfxPthreadsBarrier
+//
+/// Destructor
+//----------------------------------------------------------------------------
+PfxPthreadsBarrier::~PfxPthreadsBarrier() 
+{
+	if (m_maxThreads > 0)
+	{
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_destroy(&m_mutex));
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_destroy(&m_cond));
+	}
+}
+	
+//----------------------------------------------------------------------------
+//  PfxPthreadsBarrier::sync
+//
+/// This function is used to sync m_numThreads worker threads. Each worker
+/// should call sync() when it finishes a task. All workers will block until
+/// the last worker also calls sync().
+//----------------------------------------------------------------------------
+void PfxPthreadsBarrier::sync()
+{		
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_lock(&m_mutex));
+
+	m_called++;
+
+	if (m_called == m_maxThreads)
+	{
+		// last thread to join broadcasts a condition that will release
+		// all the threads waiting at the barrier. The barrier is reset
+		// to be ready for the next sync point.
+		m_called = 0;
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_broadcast(&m_cond));
+	}
+	else
+	{
+		// First m_numThreads - 1 worker threads block on the condition.
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_wait(&m_cond, &m_mutex));
+	}
+
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_unlock(&m_mutex));
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsBarrier::setMaxCount
+//
+/// Set the number of threads that the barrier should wait for. This also
+/// initializes a mutex and condition variable that are used to implement
+/// the barrier.
+///
+/// @param n       Number of threads that should wait at the barrier.
+//----------------------------------------------------------------------------
+void PfxPthreadsBarrier::setMaxCount(int n)
+{
+	if (m_maxThreads > 0)
+	{
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_destroy(&m_mutex));
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_destroy(&m_cond));
+	}
+
+	m_called = 0;
+
+	if (0 < n)
+	{
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_init(&m_mutex,NULL));
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_cond_init(&m_cond,NULL));
+	}
+
+	m_maxThreads = n;
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsBarrier::getMaxCount
+//
+/// Get the number of threads that the barrier will wait for.
+///
+/// @return  The number of threads the barrier waits for
+//----------------------------------------------------------------------------
+int PfxPthreadsBarrier::getMaxCount()
+{
+	return m_maxThreads;
+}
+
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsCriticalSection::PfxPthreadsCriticalSection
+//
+/// Default constructor
+//----------------------------------------------------------------------------
+PfxPthreadsCriticalSection::PfxPthreadsCriticalSection()
+{
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_init(&m_mutex,NULL));
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsCriticalSection::~PfxPthreadsCriticalSection
+//
+/// Destructor
+//----------------------------------------------------------------------------
+PfxPthreadsCriticalSection::~PfxPthreadsCriticalSection()
+{
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_destroy(&m_mutex));
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsCriticalSection::getSharedParam
+//
+/// Get the value of a shared parameter. Note that user must lock the
+/// critical section before performing this action.
+///
+/// @param i     index of shared parameter to retrieve. Must have value
+///              between 0 and 31
+///
+/// @return Shared parameter value
+//----------------------------------------------------------------------------
+PfxUInt32 PfxPthreadsCriticalSection::getSharedParam(int i)
+{
+	return(m_commonBuff[i]);
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsCriticalSection::setSharedParam
+//
+/// Set the value of a shared parameter. Note that user must lock the
+/// critical section before performing this action.
+///
+/// @param i     index of shared parameter to set. Must have value
+///              between 0 and 31
+/// @param p     Value to assign to shared parameter
+//----------------------------------------------------------------------------
+void PfxPthreadsCriticalSection::setSharedParam(int i,PfxUInt32 p)
+{
+	m_commonBuff[i] = p;
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsCriticalSection::lock
+//
+/// Lock the critical section
+//----------------------------------------------------------------------------
+void PfxPthreadsCriticalSection::lock()
+{
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_lock(&m_mutex));
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsCriticalSection::lock
+//
+/// Unlock the critical section
+//----------------------------------------------------------------------------
+void PfxPthreadsCriticalSection::unlock()
+{
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_mutex_unlock(&m_mutex));
+}
+
+//----------------------------------------------------------------------------
+// Factory functions
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+//  PfxCreateBarrierPthreads
+//
+/// Factory function to create a pthreads-based barrier
+///
+/// @param n       Max number of tasks
+//----------------------------------------------------------------------------
+PfxBarrier *PfxCreateBarrierPthreads(int n)
+{
+	PfxPthreadsBarrier *barrier = new PfxPthreadsBarrier;
+	barrier->setMaxCount(n);
+	return(barrier);
+}
+
+//----------------------------------------------------------------------------
+//  PfxCreateCriticalSectionPthreads
+//
+/// Factory function to create a pthreads-based critical section
+//----------------------------------------------------------------------------
+PfxCriticalSection *PfxCreateCriticalSectionPthreads()
+{
+	PfxPthreadsCriticalSection *cs = new PfxPthreadsCriticalSection;
+	return(cs);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif //__ANDROID__

Extras/PhysicsEffects/src/low_level/task/pfx_sync_components_pthreads.h

@@ -0,0 +1,93 @@
+/*
+ Applied Research Associates Inc. (c)2011
+
+ Redistribution and use in source and binary forms,
+   with or without modification, are permitted provided that the
+   following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Applied Research Associates Inc nor the names
+      of its contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef _SCE_PFX_SYNC_COMPONENTS_PTHREADS_H
+#define _SCE_PFX_SYNC_COMPONENTS_PTHREADS_H
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+#include "../../../include/physics_effects/low_level/task/pfx_pthreads.h"
+#include "../../../include/physics_effects/low_level/task/pfx_sync_components.h"
+#include <pthread.h>
+#include <semaphore.h>
+
+namespace sce {
+namespace PhysicsEffects {
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsBarrier
+//
+/// Implementation of a barrier using pthreads. This version uses a mutex
+/// and condition variable rather than using native pthreads barrier, which
+/// enables it to be used on platforms that don't support pthreads barriers
+/// (such as Android 2.3x)
+//----------------------------------------------------------------------------
+class PfxPthreadsBarrier : public PfxBarrier
+{
+	public:
+		PfxPthreadsBarrier();
+		virtual ~PfxPthreadsBarrier();
+
+		// from PfxBarrier
+		virtual void sync();
+		virtual void setMaxCount(int n);
+		virtual int  getMaxCount();
+
+	private:
+		pthread_mutex_t m_mutex;	///< Mutex used to block worker threads
+		pthread_cond_t m_cond;		///< Condition variable
+	
+		int m_maxThreads;			///< Maximum number of worker threads
+		int	m_called;				///< Number of worker threads waiting at barrier
+};
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsBarrier
+//
+/// Implementation of a critical section using pthreads.
+//----------------------------------------------------------------------------
+class PfxPthreadsCriticalSection : public PfxCriticalSection
+{
+	public:
+		PfxPthreadsCriticalSection();
+		virtual ~PfxPthreadsCriticalSection();
+
+		// from PfxCriticalSection
+		virtual PfxUInt32 getSharedParam(int i);
+		virtual void setSharedParam(int i,PfxUInt32 p);
+
+		virtual void lock();
+		virtual void unlock();
+
+	private:
+		pthread_mutex_t m_mutex;	///< Mutex used to implement lock
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_SYNC_COMPONENTS_PTHREADS_H

Extras/PhysicsEffects/src/low_level/task/pfx_task_manager_pthreads.cpp

@@ -0,0 +1,311 @@
+/*
+ Applied Research Associates Inc. (c)2011
+
+ Redistribution and use in source and binary forms,
+   with or without modification, are permitted provided that the
+   following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Applied Research Associates Inc nor the names
+      of its contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifdef __ANDROID__
+
+#include "../../../include/physics_effects/base_level/base/pfx_common.h"
+#include "../../../include/physics_effects/low_level/task/pfx_pthreads.h"
+#include "../../../include/physics_effects/low_level/task/pfx_task_manager.h"
+#include "../../../include/physics_effects/low_level/task/pfx_sync_components.h"
+#include "pfx_sync_components_pthreads.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+//----------------------------------------------------------------------------
+// Standalone functions and structs
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsThreadData
+//
+/// Struct to store information needed by worker threads
+//----------------------------------------------------------------------------
+struct PfxPthreadsThreadData
+{
+	// task runner information
+	PfxTaskArg *taskargument;		///< Pointer to argument for the task entry function
+	PfxTaskEntry taskEntry;			///< Pointer to current task entry function
+
+	// pthreads synchronization and thread info
+	pthread_t thread;				///< Current thread
+	sem_t semaphore;				///< Semaphore used to wake the thread
+	sem_t *taskmanagersemaphore;	///< Semaphore used to notify parent thread
+};
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsThreadFunction
+//
+/// The thread function used for threads created and managed using a
+/// PfxPthreadsThreadPool
+//----------------------------------------------------------------------------
+void *PfxPthreadsThreadFunction(void *argument) 
+{
+	PfxPthreadsThreadData *threaddata = (PfxPthreadsThreadData*)argument;
+
+	while (1)
+	{
+		// wait until a task is available
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_wait(&threaddata->semaphore));
+
+		// do work
+		if (threaddata->taskEntry)
+			threaddata->taskEntry(threaddata->taskargument);
+
+		// notify threadpool that task is done
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_post(threaddata->taskmanagersemaphore));
+
+		// If no task, then exit
+		if (!threaddata->taskEntry)
+			pthread_exit(0);
+	}
+
+	return 0;
+}
+
+//----------------------------------------------------------------------------
+// Class definitions
+//----------------------------------------------------------------------------
+class PfxPthreadsTaskManager : public PfxTaskManager
+{
+	public:
+		PfxPthreadsTaskManager(PfxUInt32 numTasks, PfxUInt32 maxTasks, void *workBuff, PfxUInt32 workBytes) :
+									PfxTaskManager(numTasks, maxTasks, workBuff, workBytes),
+									m_threads(NULL) {}
+
+		// from PfxTaskManager
+		virtual PfxUInt32 getSharedParam(int i);
+		virtual void setSharedParam(int i, PfxUInt32 p);
+		virtual void startTask(int taskId, void *io, PfxUInt32 data1, PfxUInt32 data2,
+								PfxUInt32 data3, PfxUInt32 data4);
+		virtual void waitTask(int &taskId, PfxUInt32 &data1, PfxUInt32 &data2,
+								PfxUInt32 &data3, PfxUInt32 &data4);
+
+		virtual void initialize();
+		virtual void finalize();
+
+	protected:
+		PfxPthreadsTaskManager() : PfxTaskManager(), m_threads(NULL) {}
+
+	private:
+		PfxPthreadsThreadData *m_threads;	///< Pointer to array of running threads (count is m_numThreads);
+		PfxPthreadsBarrier m_barrier;		///< Barrier used to sync task groups
+		PfxPthreadsCriticalSection m_cs;	///< Critical section used to manage shared parameters
+		sem_t m_taskmanagersemaphore;		///< Synchronization semaphore for task manager
+};
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsTaskManager
+//
+/// Implementation of a task manager using pthreads.
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsTaskManager::initialize
+//
+/// Initialize the task manager
+//----------------------------------------------------------------------------
+void PfxPthreadsTaskManager::initialize()
+{
+	if (0 == m_maxTasks)
+		return;
+
+	if (m_threads) // already started
+	{
+		SCE_PFX_PRINTF("PfxPthreadsThreadPool attempt to start threads when they are already started, line %i, file %s\n", __LINE__, __FILE__);
+		return;
+	}
+
+	m_threads = (PfxPthreadsThreadData*)m_pool.allocate(sizeof(PfxPthreadsThreadData)*m_maxTasks);
+	if (!m_threads)
+	{
+		SCE_PFX_PRINTF("PfxPthreadsThreadPool unable to allocate threads at line %i in file %s\n", __LINE__, __FILE__);
+		return;
+	}
+
+	m_barrier.setMaxCount(m_maxTasks);
+
+	// Initialize sync semaphore
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_init(&m_taskmanagersemaphore, 0, 0));
+
+	// Allocate and start the threads
+	for (unsigned int i = 0; i < m_maxTasks; i++)
+	{
+		// Prepare argument data structure for task entry functions. Mostly, this is
+		// setting parameters that are fixed until the simulation ends
+		m_taskArg[i].taskId = i;
+		m_taskArg[i].maxTasks = m_maxTasks;
+		m_taskArg[i].barrier = &m_barrier;
+		m_taskArg[i].criticalSection = &m_cs;
+		m_taskArg[i].io = NULL;
+
+		// Prepare other per-thread data
+		m_threads[i].taskEntry = NULL;
+		m_threads[i].taskargument = &m_taskArg[i];
+		m_threads[i].taskmanagersemaphore = &m_taskmanagersemaphore;
+
+		// Now create the thread's semaphore, then start the thread
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_init(&m_threads[i].semaphore, 0, 0));
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(pthread_create(&m_threads[i].thread, NULL, PfxPthreadsThreadFunction, (void*)&m_threads[i]));
+	}
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsTaskManager::finalize
+//
+/// Finalize the task manager
+//----------------------------------------------------------------------------
+void PfxPthreadsTaskManager::finalize()
+{
+	// stop the threads
+	for (unsigned int i = 0; i < m_maxTasks; i++)
+	{
+		m_threads[i].taskEntry = NULL;	// NULL task tells thread to exit
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_post(&m_threads[i].semaphore));
+	}
+
+	// wait for them all to exit
+	for (unsigned int i = 0; i < m_maxTasks; i++)
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_wait(&m_taskmanagersemaphore));
+
+	// destroy per-thread semaphores
+	for (unsigned int i = 0; i < m_maxTasks; i++)
+		SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_destroy(&m_threads[i].semaphore));
+
+	// delete thread pool
+	delete [] m_threads;
+	m_threads = NULL;
+
+	// destroy task manager semaphore and reset barrier
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_destroy(&m_taskmanagersemaphore));
+
+	m_barrier.setMaxCount(0);
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsTaskManager::startTask
+//
+/// Start a task
+///
+/// @param taskId       [in] task thread identifier/index
+/// @param io           [in, out] task input and output buffer
+/// @param data1        [in] first of four user parameter data values for the task
+/// @param data2        [in] second of four user parameter data values for the task
+/// @param data3        [in] third of four user parameter data values for the task
+/// @param data4        [in] fourth of four user parameter data values for the task
+//----------------------------------------------------------------------------
+void PfxPthreadsTaskManager::startTask(int taskId,void *io,PfxUInt32 data1,
+	PfxUInt32 data2,PfxUInt32 data3,PfxUInt32 data4)
+{
+	m_threads[taskId].taskEntry = m_taskEntry;
+	m_taskArg[taskId].io = io;
+	m_taskArg[taskId].data[0] = data1;
+	m_taskArg[taskId].data[1] = data2;
+	m_taskArg[taskId].data[2] = data3;
+	m_taskArg[taskId].data[3] = data4;
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_post(&m_threads[taskId].semaphore));
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsTaskManager::waitTask
+//
+/// Wait for a task to finish
+///
+/// @param taskId       [out] task thread identifier/index
+/// @param data1        [out] first of four data values from the task
+/// @param data2        [out] second of four data values from the task
+/// @param data3        [out] third of four data values from the task
+/// @param data4        [out] fourth of four data values from the task
+//----------------------------------------------------------------------------
+void PfxPthreadsTaskManager::waitTask(int &taskId,PfxUInt32 &data1,PfxUInt32 &data2,
+	PfxUInt32 &data3,PfxUInt32 &data4)
+{
+	SCE_PFX_CHECK_PTHREADS_OUTCOME(sem_wait(&m_taskmanagersemaphore));
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsTaskManager::getSharedParam
+//
+/// Get the value of a shared parameter
+///
+/// @param i     index of shared parameter to retrieve. Must have value
+///              between 0 and 31
+///
+/// @return Shared parameter value
+//----------------------------------------------------------------------------
+PfxUInt32 PfxPthreadsTaskManager::getSharedParam(int i)
+{
+	m_cs.lock();
+	PfxUInt32 paramval = m_cs.getSharedParam(i);
+	m_cs.unlock();
+	return(paramval);
+}
+
+//----------------------------------------------------------------------------
+//  PfxPthreadsTaskManager::setSharedParam
+//
+/// Set the value of a shared parameter
+///
+/// @param i     index of shared parameter to set. Must have value
+///              between 0 and 31
+/// @param p     Value to assign to shared parameter
+//----------------------------------------------------------------------------
+void PfxPthreadsTaskManager::setSharedParam(int i, PfxUInt32 p)
+{
+	m_cs.lock();
+	PfxUInt32 paramval = m_cs.getSharedParam(i);
+	m_cs.setSharedParam(i, p);
+	m_cs.unlock();
+}
+
+//----------------------------------------------------------------------------
+// Factory functions
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+//  PfxCreateTaskManagerPthreads
+//
+/// Factory function to create a pthreads-based task manager
+///
+/// @param numTasks       number of tasks
+/// @param maxTasks       max number of tasks
+/// @param workBuff       work buffer
+/// @param workBytes      size of work buffer, in bytes
+//----------------------------------------------------------------------------
+PfxTaskManager *PfxCreateTaskManagerPthreads(PfxUInt32 numTasks,PfxUInt32 maxTasks,
+	void *workBuff,PfxUInt32 workBytes)
+{
+	PfxTaskManager *taskmanager = new PfxPthreadsTaskManager(numTasks, maxTasks,
+																workBuff, workBytes);
+	return(taskmanager);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif //__ANDROID__

Extras/PhysicsEffects/src/util/CMakeLists.txt

@@ -0,0 +1,20 @@
+INCLUDE_DIRECTORIES( ${PHYSICS_EFFECTS_SOURCE_DIR}/include  )
+
+SET(PfxUtil_SRCS
+					pfx_mass.cpp
+					pfx_mesh_creator.cpp
+)
+
+SET(PfxUtil_HDRS
+					pfx_array.h
+					pfx_array_implementation.h
+					pfx_util_common.h
+)
+
+
+
+
+ADD_LIBRARY(PfxUtil ${PfxUtil_SRCS} ${PfxUtil_HDRS})
+
+SET_TARGET_PROPERTIES(PfxUtil PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(PfxUtil PROPERTIES SOVERSION ${BULLET_VERSION})

Extras/PhysicsEffects/src/util/pfx_array.h

@@ -0,0 +1,187 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_ARRAY_H
+#define _SCE_PFX_ARRAY_H
+
+#include "../../include/physics_effects/base_level/base/pfx_common.h"
+#include "pfx_util_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// 基本的なコンテナクラス
+
+///////////////////////////////////////////////////////////////////////////////
+// PfxArray
+
+/*
+	可変配列
+*/
+
+template <class T>
+class PfxArray
+{
+private:
+	PfxUInt32 m_numData;
+	PfxUInt32 m_maxData;
+	SCE_PFX_PADDING(1,8)
+	T SCE_PFX_ALIGNED(16) *m_data;
+	SCE_PFX_PADDING(2,12)
+	
+public:
+	PfxArray() : m_numData(0),m_maxData(100)
+	{
+		m_data = (T*)SCE_PFX_UTIL_ALLOC(16,sizeof(T)*m_maxData);
+	}
+	
+	PfxArray(PfxUInt32 maxData) : m_numData(0),m_maxData(maxData)
+	{
+		m_data = (T*)SCE_PFX_UTIL_ALLOC(16,sizeof(T)*m_maxData);
+	}
+	
+	~PfxArray()
+	{
+		SCE_PFX_UTIL_FREE(m_data);
+	}
+	
+	PfxUInt32 size() const {return m_numData;}
+	
+	inline T& operator[](PfxUInt32 i);
+	
+	inline const T& operator[](PfxUInt32 i) const;
+	
+	inline const PfxArray& operator=(const PfxArray &array);
+	
+	// 指定数分のデータを準備する
+	inline void assign(PfxUInt32 num,const T &initData);
+	
+	// データを追加
+	inline PfxUInt32 push(const T& data);
+	
+	// i番目のデータを消去
+	inline bool remove(PfxUInt32 i);
+	
+	inline bool empty() const {return m_numData==0;}
+	
+	inline void clear() {m_numData = 0;}
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// PfxQueue
+
+/*
+	キュー（最大数固定）
+*/
+
+template <class T>
+class PfxQueue
+{
+private:
+	PfxUInt32 m_numData;
+	PfxUInt32 m_maxData;
+	PfxUInt32 m_head;
+	PfxUInt32 m_tail;
+	T SCE_PFX_ALIGNED(16) *m_data;
+	SCE_PFX_PADDING(1,12)
+	
+	PfxQueue() {}
+	
+public:
+	
+	PfxQueue(PfxUInt32 maxData) : m_numData(0),m_maxData(maxData),m_head(0),m_tail(0)
+	{
+		m_data = (T*)SCE_PFX_UTIL_ALLOC(16,sizeof(T)*m_maxData);
+	}
+	
+	~PfxQueue()
+	{
+		SCE_PFX_UTIL_FREE(m_data);
+	}
+	
+	PfxUInt32 size() const {return m_numData;}
+	
+	// データをキューに入れる
+	inline PfxUInt32 push(const T& data);
+	
+	// 最後尾のデータを消去
+	inline void pop();
+	
+	// 先頭のデータを参照
+	inline T& front();
+	
+	// 先頭のデータを参照
+	inline const T& front() const;
+	
+	inline bool empty() const {return m_numData==0;}
+	
+	inline void clear() {m_numData = 0;}
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// PfxStack
+
+/*
+	スタック（最大数固定）
+*/
+
+template <class T>
+class PfxStack
+{
+private:
+	PfxUInt32 m_numData;
+	PfxUInt32 m_maxData;
+	T SCE_PFX_ALIGNED(16) *m_data;
+	
+	PfxStack() {}
+	
+public:
+
+	PfxStack(PfxUInt32 maxData) : m_numData(0),m_maxData(maxData)
+	{
+		m_data = (T*)SCE_PFX_UTIL_ALLOC(16,sizeof(T)*m_maxData);
+	}
+	
+	~PfxStack()
+	{
+		SCE_PFX_UTIL_FREE(m_data);
+	}
+	
+	PfxUInt32 size() const {return m_numData;}
+	
+	// データをスタックに入れる
+	inline PfxUInt32 push(const T& data);
+	
+	// 末尾のデータを消去
+	inline void pop();
+	
+	// 末尾のデータを参照
+	inline T& top();
+	
+	// 末尾のデータを参照
+	inline const T& top() const;
+	
+	inline bool empty() const {return m_numData==0;}
+	
+	inline void clear() {m_numData = 0;}
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#include "pfx_array_implementation.h"
+
+#endif // _SCE_PFX_ARRAY_H

Extras/PhysicsEffects/src/util/pfx_array_implementation.h

@@ -0,0 +1,159 @@
+/*
+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
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// PfxArray
+
+namespace sce {
+namespace PhysicsEffects {
+template <class T>
+inline T& PfxArray<T>::operator[](PfxUInt32 i)
+{
+	return m_data[i];
+}
+
+template <class T>
+inline const T& PfxArray<T>::operator[](PfxUInt32 i) const
+{
+	return m_data[i];
+}
+
+template <class T>
+inline const PfxArray<T>& PfxArray<T>::operator=(const PfxArray &array)
+{
+	clear();
+	if(array.size() > m_maxData) {
+		m_maxData = array.size();
+		m_data = (T*)SCE_PFX_UTIL_REALLOC(m_data,16,sizeof(T)*m_maxData);
+		SCE_PFX_ASSERT(m_data);
+	}
+	for(PfxUInt32 i=0;i<array.size();i++) {
+		push(array[i]);
+	}
+	return *this;
+}
+
+template <class T>
+inline void PfxArray<T>::assign(PfxUInt32 num,const T &initData)
+{
+	clear();
+	if(num > m_maxData) {
+		m_maxData = num;
+		m_data = (T*)SCE_PFX_UTIL_REALLOC(m_data,16,sizeof(T)*m_maxData);
+		SCE_PFX_ASSERT(m_data);
+	}
+	for(PfxUInt32 i=0;i<num;i++) {
+		push(initData);
+	}
+}
+
+template <class T>
+inline PfxUInt32 PfxArray<T>::push(const T& data)
+{
+	if(m_numData == m_maxData) {
+		m_maxData<<=1;
+		m_data = (T*)SCE_PFX_UTIL_REALLOC(m_data,16,sizeof(T)*m_maxData);
+		SCE_PFX_ASSERT(m_data);
+	}
+	
+	PfxUInt32 id = m_numData++;
+	m_data[id] = data;
+	return id;
+}
+
+template <class T>
+inline bool PfxArray<T>::remove(PfxUInt32 i)
+{
+	if(i>=m_numData) {
+		return false;
+	}
+	
+	m_numData--;
+	m_data[i] = m_data[m_numData];
+	
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// PfxQueue
+
+template <class T>
+inline PfxUInt32 PfxQueue<T>::push(const T& data)
+{
+	SCE_PFX_ASSERT(m_numData<m_maxData);
+	PfxUInt32 id = m_tail;
+	m_data[id] = data;
+	m_tail = (m_tail+1)%m_maxData;
+	m_numData++;
+	return id;
+}
+
+template <class T>
+inline void PfxQueue<T>::pop()
+{
+	SCE_PFX_ASSERT(m_numData>0);
+	m_head = (m_head+1)%m_maxData;
+	m_numData--;
+}
+
+template <class T>
+inline T& PfxQueue<T>::front()
+{
+	SCE_PFX_ASSERT(m_numData>0);
+	return m_data[m_head];
+}
+
+template <class T>
+inline const T& PfxQueue<T>::front() const
+{
+	SCE_PFX_ASSERT(m_numData>0);
+	return m_data[m_head];
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// PfxStack
+
+template <class T>
+inline PfxUInt32 PfxStack<T>::push(const T& data)
+{
+	SCE_PFX_ASSERT(m_numData<m_maxData);
+	PfxUInt32 id = m_numData++;
+	m_data[id] = data;
+	return id;
+}
+
+template <class T>
+inline void PfxStack<T>::pop()
+{
+	SCE_PFX_ASSERT(m_numData>0);
+	m_numData--;
+}
+
+template <class T>
+inline T& PfxStack<T>::top()
+{
+	SCE_PFX_ASSERT(m_numData>0);
+	return m_data[m_numData-1];
+}
+
+template <class T>
+inline const T& PfxStack<T>::top() const
+{
+	SCE_PFX_ASSERT(m_numData>0);
+	return m_data[m_numData-1];
+}
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/util/pfx_mass.cpp

@@ -0,0 +1,102 @@
+/*
+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/util/pfx_mass.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Box
+
+PfxFloat pfxCalcMassBox(PfxFloat density,const PfxVector3 &halfExtent)
+{
+	return density * halfExtent[0] * halfExtent[1] * halfExtent[2] * 8;
+}
+
+PfxMatrix3 pfxCalcInertiaBox(const PfxVector3 &halfExtent,PfxFloat mass)
+{
+	PfxVector3 sqrSz = halfExtent * 2.0f;
+	sqrSz = mulPerElem(sqrSz,sqrSz);
+	PfxMatrix3 inertia = PfxMatrix3::identity();
+	inertia[0][0] = (mass*(sqrSz[1]+sqrSz[2]))/12.0f;
+	inertia[1][1] = (mass*(sqrSz[0]+sqrSz[2]))/12.0f;
+	inertia[2][2] = (mass*(sqrSz[0]+sqrSz[1]))/12.0f;
+	return inertia;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Sphere
+
+PfxFloat pfxCalcMassSphere(PfxFloat density,PfxFloat radius)
+{
+	return (4.0f/3.0f) * SCE_PFX_PI * radius * radius * radius * density;
+}
+
+PfxMatrix3 pfxCalcInertiaSphere(PfxFloat radius,PfxFloat mass)
+{
+	PfxMatrix3 inertia = PfxMatrix3::identity();
+	inertia[0][0] = inertia[1][1] = inertia[2][2] = 0.4f * mass * radius * radius;
+	return inertia;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Cylinder
+
+PfxFloat pfxCalcMassCylinder(PfxFloat density,PfxFloat halfLength,PfxFloat radius)
+{
+	return SCE_PFX_PI * radius * radius * 2.0f * halfLength * density;
+}
+
+static inline
+PfxMatrix3 pfxCalcInertiaCylinder(PfxFloat halfLength,PfxFloat radius,PfxFloat mass,int axis)
+{
+	PfxMatrix3 inertia = PfxMatrix3::identity();
+	inertia[0][0] = inertia[1][1] = inertia[2][2] = 0.25f * mass * radius * radius + 0.33f * mass * halfLength * halfLength; 
+	inertia[axis][axis] = 0.5f * mass * radius * radius;
+	return inertia;
+}
+
+PfxMatrix3 pfxCalcInertiaCylinderX(PfxFloat halfLength,PfxFloat radius,PfxFloat mass)
+{
+	return pfxCalcInertiaCylinder(radius,halfLength,mass,0);
+}
+
+PfxMatrix3 pfxCalcInertiaCylinderY(PfxFloat halfLength,PfxFloat radius,PfxFloat mass)
+{
+	return pfxCalcInertiaCylinder(radius,halfLength,mass,1);
+}
+
+PfxMatrix3 pfxCalcInertiaCylinderZ(PfxFloat halfLength,PfxFloat radius,PfxFloat mass)
+{
+	return pfxCalcInertiaCylinder(radius,halfLength,mass,2);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+PfxMatrix3 pfxMassTranslate(PfxFloat mass,const PfxMatrix3 &inertia,const PfxVector3 &translation)
+{
+	PfxMatrix3 m = crossMatrix(translation);
+	return inertia + mass * (-m*m);
+}
+
+PfxMatrix3 pfxMassRotate(const PfxMatrix3 &inertia,const PfxMatrix3 &rotate)
+{
+	return rotate * inertia * transpose(rotate);
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/util/pfx_mesh_creator.cpp

@@ -0,0 +1,944 @@
+/*
+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/util/pfx_mesh_creator.h"
+#include "pfx_array.h"
+#include "../base_level/collision/pfx_intersect_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_LARGETRIMESH_MAX_ISLANDS 256
+
+///////////////////////////////////////////////////////////////////////////////
+// 凸メッシュ作成時に使用する関数
+
+PfxInt32 pfxCreateConvexMesh(PfxConvexMesh &convex,const PfxCreateConvexMeshParam &param)
+{
+	// Check input
+	if(param.numVerts == 0 || param.numTriangles == 0 || !param.verts || !param.triangles)
+		return SCE_PFX_ERR_INVALID_VALUE;
+	
+	if(param.numVerts >= SCE_PFX_NUMMESHVERTICES || param.numTriangles >= SCE_PFX_NUMMESHFACETS)
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	
+	PfxArray<PfxVector3> vertList;
+	for(PfxUInt32 i=0;i<param.numVerts;i++) {
+		PfxFloat *vtx = (PfxFloat*)((uintptr_t)param.verts + param.vertexStrideBytes * i);
+		vertList.push(PfxVector3(vtx[0],vtx[1],vtx[2]));
+	}
+
+	PfxArray<PfxUInt32> facetList;
+	for(PfxUInt32 i=0;i<param.numTriangles;i++) {
+		void *ids = (void*)((uintptr_t)param.triangles + param.triangleStrideBytes * i);
+		PfxUInt32 idx[3];
+		
+		if(param.flag & SCE_PFX_MESH_FLAG_32BIT_INDEX) {
+			if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
+				idx[0] = ((PfxUInt32*)ids)[2];
+				idx[1] = ((PfxUInt32*)ids)[1];
+				idx[2] = ((PfxUInt32*)ids)[0];
+			}
+			else {
+				idx[0] = ((PfxUInt32*)ids)[0];
+				idx[1] = ((PfxUInt32*)ids)[1];
+				idx[2] = ((PfxUInt32*)ids)[2];
+			}
+		}
+		else if(param.flag & SCE_PFX_MESH_FLAG_16BIT_INDEX) {
+			if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
+				idx[0] = ((PfxUInt16*)ids)[2];
+				idx[1] = ((PfxUInt16*)ids)[1];
+				idx[2] = ((PfxUInt16*)ids)[0];
+			}
+			else {
+				idx[0] = ((PfxUInt16*)ids)[0];
+				idx[1] = ((PfxUInt16*)ids)[1];
+				idx[2] = ((PfxUInt16*)ids)[2];
+			}
+		}
+		else {
+			return SCE_PFX_ERR_INVALID_FLAG;
+		}
+
+		// 面積が０の面を排除
+		PfxFloat area = lengthSqr(cross(vertList[idx[1]]-vertList[idx[0]],vertList[idx[2]]-vertList[idx[0]]));
+
+		if((param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) && area < 0.00001f) {
+			continue;
+		}
+		
+		facetList.push(idx[0]);
+		facetList.push(idx[1]);
+		facetList.push(idx[2]);
+	}
+
+	convex.m_numVerts = vertList.size();
+	for(PfxUInt32 i=0;i<convex.m_numVerts;i++) {
+		convex.m_verts[i] = vertList[i];
+	}
+
+	convex.m_numIndices = facetList.size();
+	for(PfxUInt32 i=0;i<convex.m_numIndices;i++) {
+		convex.m_indices[i] = facetList[i];
+	}
+
+	convex.updateAABB();
+
+	return SCE_PFX_OK;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// ラージメッシュ作成時に使用する構造体
+
+struct PfxMcVert {
+	PfxInt16 i;
+	PfxInt16 flag;
+	SCE_PFX_PADDING(1,12)
+	PfxVector3  coord;
+};
+
+struct PfxMcEdge {
+	PfxUInt32 vertId[2];  // 
+	PfxUInt32 edgeId[2];  // 面におけるエッジのインデックス
+	PfxUInt32 facetId[2]; // 
+	PfxUInt32 numFacets;
+	PfxUInt32 angleType;  // 辺の種類
+	PfxFloat angle;		 // 辺の角度
+	PfxMcEdge *next;
+};
+
+struct PfxMcFacet {
+	PfxMcVert *v[3];
+	PfxMcEdge *e[3];
+	PfxInt32 neighbor[3];	// 隣接面のインデックス
+	PfxInt32 neighborEdgeId[3];	// 隣接面のエッジインデックス
+	PfxFloat thickness;	// 厚み
+	PfxFloat area;			// 面積
+	SCE_PFX_PADDING(1,8)
+	PfxVector3 n;
+	PfxVector3 aabbMin;
+	PfxVector3 aabbMax;
+};
+
+struct PfxMcTriList {
+	PfxMcFacet *facet;
+	PfxMcTriList *next;
+
+	PfxMcTriList()
+	{
+		facet = NULL;
+		next = NULL;
+	}
+};
+
+struct PfxMcEdgeEntry {
+	PfxUInt8 vertId[2];
+	PfxUInt8 facetId[2];
+	PfxUInt8 numFacets;
+	PfxUInt8 edgeNum[2];
+	PfxUInt8 edgeId;
+	SCE_PFX_PADDING(1,8)
+	PfxVector3 dir;
+	PfxMcEdgeEntry *next;
+	SCE_PFX_PADDING(2,12)
+};
+
+struct PfxMcFacetLink {
+	PfxInt32 baseEdgeId;
+	PfxInt32 vid1;
+	PfxInt32 vid2;
+	PfxInt32 ifacetId;
+	PfxInt32 iedgeId;
+	PfxInt32 ofacetId;
+	PfxInt32 oedgeId;
+	
+	PfxMcFacetLink(PfxInt32 baseEdgeId_,PfxInt32 vid1_,PfxInt32 vid2_,PfxInt32 ifacetId_,PfxInt32 iedgeId_,PfxInt32 ofacetId_,PfxInt32 oedgeId_)
+	{
+		baseEdgeId = baseEdgeId_;
+		vid1 = vid1_;
+		vid2 = vid2_;
+		ifacetId = ifacetId_;
+		iedgeId = iedgeId_;
+		ofacetId = ofacetId_;
+		oedgeId = oedgeId_;
+	};
+};
+
+typedef PfxMcFacet* PfxMcFacetPtr;
+
+struct PfxMcIslands {
+	PfxArray<PfxMcFacetPtr> facetsInIsland[SCE_PFX_MAX_LARGETRIMESH_ISLANDS];
+	PfxUInt32 numIslands;
+	SCE_PFX_PADDING(1,12)
+	
+	PfxMcIslands()
+	{
+		numIslands = 0;
+	}
+	
+	void add(PfxArray<PfxMcFacetPtr> &facets)
+	{
+		facetsInIsland[numIslands++] = facets;
+	}
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// ラージメッシュ作成時に使用する補助関数
+
+static
+bool intersect(const PfxMcFacet &facetA,const PfxMcFacet &facetB,PfxFloat &closestDistance)
+{
+	const PfxFloat epsilon = 0.00001f;
+
+	PfxVector3 pA[3] = {
+		facetA.v[0]->coord,
+		facetA.v[1]->coord,
+		facetA.v[2]->coord
+	};
+	
+	PfxVector3 pB[3] = {
+		facetB.v[0]->coord,
+		facetB.v[1]->coord,
+		facetB.v[2]->coord
+	};
+
+	// 面Bが面Aの厚みを考慮した範囲内に有るかどうかチェック
+
+	// 上下面
+	{
+		PfxPlane planeA(facetA.n,pA[0]);
+		PfxFloat dmin = SCE_PFX_FLT_MAX;
+		PfxFloat dmax = -SCE_PFX_FLT_MAX;
+		for(int i=0;i<3;i++) {
+			PfxFloat d = planeA.onPlane(pB[i]);
+			dmin = SCE_PFX_MIN(dmin,d);
+			dmax = SCE_PFX_MAX(dmax,d);
+		}
+		
+		if(dmin > -epsilon || dmax < -facetA.thickness) return false;
+		
+		// 面Aと面Bの最近接距離
+		if(dmax > 0.0f) {
+			// 面A,Bは交差
+			return false;
+		}
+		else if(dmax > -epsilon) {
+			// 隣接面
+			return false;
+		}
+		else {
+			closestDistance = -dmax;
+		}
+	}
+	
+	// 側面
+	for(int p=0;p<3;p++) {
+		PfxVector3 sideVec = normalize(cross((pA[(p+1)%3]-pA[p]),facetA.n));
+		PfxPlane planeA(sideVec,pA[p]);
+
+		PfxFloat dmin = SCE_PFX_FLT_MAX;
+		for(int i=0;i<3;i++) {
+			PfxFloat d = planeA.onPlane(pB[i]);
+			dmin = SCE_PFX_MIN(dmin,d);
+		}
+		
+		if(dmin > -epsilon) return false;
+	}
+	
+	return true;
+}
+
+static
+void divideMeshes(
+	PfxUInt32 numFacetsLimit,PfxFloat islandsRatio,
+	PfxMcIslands &islands,
+	PfxArray<PfxMcFacetPtr> &facets,
+	const PfxVector3 &center,const PfxVector3 &half)
+{
+	PfxFloat halfLimit = length(half) * islandsRatio;
+	
+	// 含まれる面数が規定値以下であれば、アイランドに登録
+	if((facets.size() <= SCE_PFX_NUMMESHFACETS && length(half) < halfLimit) || 
+		(facets.size() <= numFacetsLimit ) ) {
+		islands.add(facets);
+		return;
+	}
+
+	// さらに分割
+	PfxVector3 newCenter0,newCenter1;
+	PfxVector3 newHalf0,newHalf1;
+	PfxArray<PfxMcFacetPtr> newFacets0;
+	PfxArray<PfxMcFacetPtr> newFacets1;
+
+	// 最も適切と思われる分離軸を探す
+	int divAxis;
+	{
+		if(half[0] > half[1]) {
+			if(half[0] > half[2]) {
+				divAxis = 0;
+			}
+			else if(half[1] > half[2]) {
+				divAxis = 1;
+			}
+			else {
+				divAxis = 2;
+			}
+		}
+		else {
+			if(half[1] > half[2]) {
+				divAxis = 1;
+			}
+			else if(half[0] > half[2]) {
+				divAxis = 0;
+			}
+			else {
+				divAxis = 2;
+			}
+		}
+	}
+
+	// 中心で分割して、さらに再帰的に処理を続ける
+	{
+		PfxVector3 movCenter(0.0f);
+		movCenter[divAxis] = 0.5f*half[divAxis];
+		
+		newCenter0 = center + movCenter;
+		newCenter1 = center - movCenter;
+		newHalf0 = half;
+		newHalf0[divAxis] *= 0.5f;
+		newHalf1 = newHalf0;
+	}
+	
+	// 新しいAABBに含まれる面をそれぞれの領域に分配
+	for(PfxUInt32 f=0;f<facets.size();f++) {
+		// 面のAABB
+		PfxVector3 facetCenter = (facets[f]->aabbMin + facets[f]->aabbMax) * 0.5f;
+		PfxVector3 facetHalf = (facets[f]->aabbMax - facets[f]->aabbMin) * 0.5f;
+		
+		// AABB判定
+		if(!(fabsf(newCenter0[divAxis]-facetCenter[divAxis]) > (newHalf0[divAxis]+facetHalf[divAxis]))) {
+			// この面はAABB0に登録
+			newFacets0.push(facets[f]);
+		}
+		else {
+			// この面はAABB1に登録
+			newFacets1.push(facets[f]);
+		}
+	}
+
+	if(newFacets0.size() < newFacets1.size()) {
+		if(newFacets0.size() > 0)
+			divideMeshes(numFacetsLimit,islandsRatio,islands,newFacets0,newCenter0,newHalf0);
+		if(newFacets1.size() > 0)
+			divideMeshes(numFacetsLimit,islandsRatio,islands,newFacets1,newCenter1,newHalf1);
+	}
+	else {
+		if(newFacets1.size() > 0)
+			divideMeshes(numFacetsLimit,islandsRatio,islands,newFacets1,newCenter1,newHalf1);
+		if(newFacets0.size() > 0)
+			divideMeshes(numFacetsLimit,islandsRatio,islands,newFacets0,newCenter0,newHalf0);
+	}
+}
+
+static
+int addIslandToLargeTriMesh(PfxLargeTriMesh &lmesh,PfxTriMesh &island)
+{
+	SCE_PFX_ASSERT(island.m_numFacets <= SCE_PFX_NUMMESHFACETS);
+
+	int newIsland = lmesh.m_numIslands++;
+	lmesh.m_islands[newIsland] = island;
+	
+	// アイランドローカルのAABBを計算
+	if(island.m_numFacets > 0) {
+		PfxVector3 aabbMin(SCE_PFX_FLT_MAX),aabbMax(-SCE_PFX_FLT_MAX);
+
+		for(PfxUInt32 i=0;i<island.m_numFacets;i++) {
+			aabbMin = minPerElem(aabbMin,pfxReadVector3(island.m_facets[i].m_center)-pfxReadVector3(island.m_facets[i].m_half));
+			aabbMax = maxPerElem(aabbMax,pfxReadVector3(island.m_facets[i].m_center)+pfxReadVector3(island.m_facets[i].m_half));
+		}
+
+		PfxVecInt3 aabbMinL,aabbMaxL;
+		lmesh.getLocalPosition(aabbMin,aabbMax,aabbMinL,aabbMaxL);
+
+		pfxSetXMin(lmesh.m_aabbList[newIsland],aabbMinL.getX());
+		pfxSetXMax(lmesh.m_aabbList[newIsland],aabbMaxL.getX());
+		pfxSetYMin(lmesh.m_aabbList[newIsland],aabbMinL.getY());
+		pfxSetYMax(lmesh.m_aabbList[newIsland],aabbMaxL.getY());
+		pfxSetZMin(lmesh.m_aabbList[newIsland],aabbMinL.getZ());
+		pfxSetZMax(lmesh.m_aabbList[newIsland],aabbMaxL.getZ());
+	}
+	
+	return newIsland;
+}
+
+static
+void createIsland(PfxTriMesh &island,const PfxArray<PfxMcFacetPtr> &facets)
+{
+	if(facets.empty()) return;
+	
+	island.m_numFacets = facets.size();
+	
+	PfxUInt32 vertsFlag[(0xff*SCE_PFX_NUMMESHFACETS*3+31)/32];
+	memset(vertsFlag,0,sizeof(PfxUInt32)*((0xff*SCE_PFX_NUMMESHFACETS*3+31)/32));
+	
+	PfxArray<PfxMcEdgeEntry*> edgeHead(facets.size()*3);
+	PfxArray<PfxMcEdgeEntry> edgeList(facets.size()*3);
+
+	PfxMcEdgeEntry* nl = NULL;
+	edgeHead.assign(facets.size()*3,nl);
+	edgeList.assign(facets.size()*3,PfxMcEdgeEntry());
+	
+	int vcnt = 0;
+	int ecnt = 0;
+	for(PfxUInt32 f=0;f<facets.size();f++) {
+		PfxMcFacet &iFacet = *facets[f];
+		PfxMcEdge *iEdge[3] = {
+			iFacet.e[0],
+			iFacet.e[1],
+			iFacet.e[2],
+		};
+
+		PfxFacet &oFacet = island.m_facets[f];
+
+		oFacet.m_half[0] = oFacet.m_half[1] = oFacet.m_half[2] = 0.0f;
+		oFacet.m_center[0] = oFacet.m_center[1] = oFacet.m_center[2] = 0.0f;
+		pfxStoreVector3(iFacet.n,oFacet.m_normal);
+		oFacet.m_thickness = iFacet.thickness;
+		
+		// Vertex
+		for(int v=0;v<3;v++) {
+			PfxMcVert *vert = facets[f]->v[v];
+			PfxUInt32 idx = vert->i;
+			PfxUInt32 mask = 1 << (idx & 31);
+			if((vertsFlag[idx>>5] & mask) == 0) {
+				SCE_PFX_ASSERT(vcnt<SCE_PFX_NUMMESHVERTICES);
+                vertsFlag[idx>>5] |= mask;
+                island.m_verts[vcnt] = vert->coord;
+				vert->flag = vcnt;// 新しいインデックス
+				vcnt++;
+			}
+			oFacet.m_vertIds[v] = (PfxUInt8)vert->flag;
+		}
+		
+		// Edge
+		for(int v=0;v<3;v++) {
+			PfxUInt8 viMin = SCE_PFX_MIN(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
+			PfxUInt8 viMax = SCE_PFX_MAX(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
+			int key = ((0x8da6b343*viMin+0xd8163841*viMax)%(island.m_numFacets*3));
+			for(PfxMcEdgeEntry *e=edgeHead[key];;e=e->next) {
+				if(!e) {
+					edgeList[ecnt].vertId[0] = viMin;
+					edgeList[ecnt].vertId[1] = viMax;
+					edgeList[ecnt].facetId[0] = f;
+					edgeList[ecnt].numFacets = 1;
+					edgeList[ecnt].edgeNum[0] = v;
+					edgeList[ecnt].edgeId = ecnt;
+					edgeList[ecnt].dir = normalize(island.m_verts[viMax]-island.m_verts[viMin]);
+					edgeList[ecnt].next = edgeHead[key];
+					edgeHead[key] = &edgeList[ecnt];
+					
+					PfxEdge edge;
+					edge.m_angleType = iEdge[v]->angleType;
+					// 厚み角の設定 0～πを0～255の整数値に変換して格納
+					edge.m_tilt = (PfxUInt8)((iEdge[v]->angle/(0.5f*SCE_PFX_PI))*255.0f);
+					edge.m_vertId[0] = viMin;
+					edge.m_vertId[1] = viMax;
+					
+					oFacet.m_edgeIds[v] = ecnt;
+					island.m_edges[ecnt] = edge;
+					SCE_PFX_ASSERT(ecnt <= SCE_PFX_NUMMESHEDGES);
+					ecnt++;
+					break;
+				}
+				
+				if(e->vertId[0] == viMin && e->vertId[1] == viMax) {
+					SCE_PFX_ASSERT(e->numFacets==1);
+                    e->facetId[1] = f;
+					e->edgeNum[1] = v;
+					e->numFacets = 2;
+					oFacet.m_edgeIds[v] = e->edgeId;
+					break;
+				}
+			}
+		}
+	}
+
+	island.m_numEdges = ecnt;
+	island.m_numVerts = vcnt;
+	
+	island.updateAABB();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// ラージメッシュ
+
+PfxInt32 pfxCreateLargeTriMesh(PfxLargeTriMesh &lmesh,const PfxCreateLargeTriMeshParam &param)
+{
+	// Check input
+	if(param.numVerts == 0 || param.numTriangles == 0 || !param.verts || !param.triangles)
+		return SCE_PFX_ERR_INVALID_VALUE;
+	
+	if(param.islandsRatio < 0.0f || param.islandsRatio > 1.0f)
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	
+	if(param.numFacetsLimit == 0 || param.numFacetsLimit > SCE_PFX_NUMMESHFACETS)
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	
+	const PfxFloat epsilon = 0.00001f;
+	
+	PfxArray<PfxMcVert>  vertList(param.numVerts);		// 頂点配列
+	PfxArray<PfxMcFacet> facetList(param.numTriangles);	// 面配列
+	PfxArray<PfxMcEdge>  edgeList(param.numTriangles*3);	// エッジ配列
+	PfxArray<PfxMcEdge*> edgeHead(param.numTriangles*3);
+	
+	//J 頂点配列作成
+	for(PfxUInt32 i=0;i<param.numVerts;i++) {
+		PfxFloat *vtx = (PfxFloat*)((uintptr_t)param.verts + param.vertexStrideBytes * i);
+		PfxMcVert mcv;
+		mcv.flag = 0;
+		mcv.i = i;
+		mcv.coord = pfxReadVector3(vtx);
+		vertList.push(mcv);
+	}
+	
+	// 面配列作成
+	for(PfxUInt32 i=0;i<param.numTriangles;i++) {
+		void *ids = (void*)((uintptr_t)param.triangles + param.triangleStrideBytes * i);
+		
+		PfxUInt32 idx[3];
+		
+		if(param.flag & SCE_PFX_MESH_FLAG_32BIT_INDEX) {
+			if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
+				idx[0] = ((PfxUInt32*)ids)[2];
+				idx[1] = ((PfxUInt32*)ids)[1];
+				idx[2] = ((PfxUInt32*)ids)[0];
+			}
+			else {
+				idx[0] = ((PfxUInt32*)ids)[0];
+				idx[1] = ((PfxUInt32*)ids)[1];
+				idx[2] = ((PfxUInt32*)ids)[2];
+			}
+		}
+		else if(param.flag & SCE_PFX_MESH_FLAG_16BIT_INDEX) {
+			if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
+				idx[0] = ((PfxUInt16*)ids)[2];
+				idx[1] = ((PfxUInt16*)ids)[1];
+				idx[2] = ((PfxUInt16*)ids)[0];
+			}
+			else {
+				idx[0] = ((PfxUInt16*)ids)[0];
+				idx[1] = ((PfxUInt16*)ids)[1];
+				idx[2] = ((PfxUInt16*)ids)[2];
+			}
+		}
+		else {
+			return SCE_PFX_ERR_INVALID_FLAG;
+		}
+		
+		const PfxVector3 pnts[3] = {
+			vertList[idx[0]].coord,
+			vertList[idx[1]].coord,
+			vertList[idx[2]].coord,
+		};
+
+		// 面積が０の面を排除
+		PfxFloat area = lengthSqr(cross(pnts[1]-pnts[0],pnts[2]-pnts[0]));
+
+		if((param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) && area < 0.00001f) {
+			continue;
+		}
+
+		PfxMcFacet facet;
+		facet.v[0] = &vertList[idx[0]];
+		facet.v[1] = &vertList[idx[1]];
+		facet.v[2] = &vertList[idx[2]];
+		facet.e[0] = facet.e[1] = facet.e[2] = NULL;
+		facet.n = normalize(cross(pnts[2]-pnts[1],pnts[0]-pnts[1]));
+		facet.area = area;
+		facet.thickness = param.defaultThickness;
+		facet.neighbor[0] = facet.neighbor[1] = facet.neighbor[2] = -1;
+		facet.neighborEdgeId[0] = facet.neighborEdgeId[1] = facet.neighborEdgeId[2] = -1;
+
+		facetList.push(facet);
+	}
+
+	const PfxUInt32 numTriangles = facetList.size();
+
+	{
+		PfxArray<PfxMcTriList> triEntry(numTriangles*3);
+		PfxArray<PfxMcTriList*> triHead(numTriangles*3);	// 頂点から面への参照リスト
+		PfxInt32 cnt = 0;
+		
+		PfxMcTriList* nl = NULL;
+		triEntry.assign(numTriangles*3,PfxMcTriList());
+		triHead.assign(numTriangles*3,nl);
+		
+		// 頂点から面への参照リストを作成
+		for(PfxUInt32 i=0;i<numTriangles;i++) {
+			for(PfxUInt32 v=0;v<3;v++) {
+				PfxUInt32 vertId = facetList[i].v[v]->i;
+				triEntry[cnt].facet = &facetList[i];
+				triEntry[cnt].next = triHead[vertId];
+				triHead[vertId] = &triEntry[cnt++];
+			}
+		}
+		
+		// 同一頂点をまとめる
+		if(param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) {
+			for(PfxUInt32 i=0;i<param.numVerts;i++) {
+				if(vertList[i].flag == 1) continue;
+				for(PfxUInt32 j=i+1;j<param.numVerts;j++) {
+					if(vertList[j].flag == 1) continue;
+
+					PfxFloat lenSqr = lengthSqr(vertList[i].coord-vertList[j].coord);
+					
+					if(lenSqr < epsilon) {
+						//SCE_PFX_PRINTF("same position %d,%d\n",i,j);
+						vertList[j].flag = 1; // 同一点なのでフラグを立てる
+						for(PfxMcTriList *f=triHead[j];f!=NULL;f=f->next) {
+							for(PfxInt32 k=0;k<3;k++) {
+								if(f->facet->v[k] == &vertList[j]) {
+									f->facet->v[k] = &vertList[i]; // 頂点を付け替える
+									break;
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+
+	// 接続面間の角度を算出して面にセット
+	PfxMcEdge *nl = NULL;
+	edgeHead.assign(numTriangles*3,nl);
+	edgeList.assign(numTriangles*3,PfxMcEdge());
+	
+	// エッジ配列の作成
+	PfxUInt32 ecnt = 0;
+	for(PfxUInt32 i=0;i<numTriangles;i++) {
+		PfxMcFacet &f = facetList[i];
+		
+		for(PfxUInt32 v=0;v<3;v++) {
+			uintptr_t vp1 = ((uintptr_t)f.v[v]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert);
+			uintptr_t vp2 = ((uintptr_t)f.v[(v+1)%3]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert);
+			PfxUInt32 viMin = SCE_PFX_MIN(vp1,vp2);
+			PfxUInt32 viMax = SCE_PFX_MAX(vp1,vp2);
+			PfxInt32 key = ((0x8da6b343*viMin+0xd8163841*viMax)%(numTriangles*3));
+			for(PfxMcEdge *e = edgeHead[key];;e=e->next) {
+				if(!e) {
+					edgeList[ecnt].vertId[0] = viMin;
+					edgeList[ecnt].vertId[1] = viMax;
+					edgeList[ecnt].facetId[0] = i;
+					edgeList[ecnt].edgeId[0] = v;
+					edgeList[ecnt].numFacets = 1;
+					edgeList[ecnt].next = edgeHead[key];
+					edgeList[ecnt].angleType = SCE_PFX_EDGE_CONVEX;
+					edgeList[ecnt].angle = 0.0f;
+					edgeHead[key] = &edgeList[ecnt];
+					f.e[v] = &edgeList[ecnt];
+					ecnt++;
+					break;
+				}
+				
+				if(e->vertId[0] == viMin && e->vertId[1] == viMax) {
+					SCE_PFX_ALWAYS_ASSERT_MSG(e->numFacets == 1,"An edge connected with over 2 triangles is invalid");
+                    e->facetId[1] = i;
+					e->edgeId[1] = v;
+					e->numFacets = 2;
+					f.e[v] = e;
+					f.neighbor[v] = e->facetId[0];
+					f.neighborEdgeId[v] = e->edgeId[0];
+					facetList[e->facetId[0]].neighbor[e->edgeId[0]] = i;
+					facetList[e->facetId[0]].neighborEdgeId[e->edgeId[0]] = e->edgeId[1];
+					break;
+				}
+			}
+		}
+	}
+	
+	// 角度を計算
+	for(PfxUInt32 i=0;i<numTriangles;i++) {
+		PfxMcFacet &facetA = facetList[i];
+
+		PfxQueue<PfxMcFacetLink> cqueue(ecnt);
+
+		for(PfxUInt32 j=0;j<3;j++) {
+			if(facetA.neighbor[j] >= 0) {
+				cqueue.push(PfxMcFacetLink(
+					j,
+					facetA.e[j]->vertId[0],facetA.e[j]->vertId[1],
+					i,j,
+					facetA.neighbor[j],facetA.neighborEdgeId[j]));
+			}
+		}
+
+		while(!cqueue.empty()) {
+			PfxMcFacetLink link = cqueue.front();
+			cqueue.pop();
+			
+			PfxMcFacet &ofacet = facetList[link.ofacetId];
+			PfxMcEdge *edge = ofacet.e[link.oedgeId];
+			
+			// facetAとのなす角を計算
+			{
+				// 面に含まれるが、このエッジに含まれない点
+				PfxUInt32 ids[3] = {2,0,1};
+				PfxVector3 v1 = facetA.v[ids[link.baseEdgeId]]->coord;
+				PfxVector3 v2 = ofacet.v[ids[link.oedgeId]]->coord;
+				
+				// エッジの凹凸判定
+				PfxVector3 midPnt = (v1 + v2) * 0.5f;
+				PfxVector3 pntOnEdge = facetA.v[link.baseEdgeId]->coord;
+				
+				PfxFloat chk1 = dot(facetA.n,midPnt-pntOnEdge);
+				PfxFloat chk2 = dot(ofacet.n,midPnt-pntOnEdge);
+				
+				if(chk1 < -epsilon && chk2 < -epsilon) {
+					if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONVEX;
+
+					// 厚み角の判定に使う角度をセット
+					if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
+						edge->angle = 0.5f*acosf(dot(facetA.n,ofacet.n));
+					}
+				}
+				else if(chk1 > epsilon && chk2 > epsilon) {
+					if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONCAVE;
+				}
+				else {
+					if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_FLAT;
+				}
+			}
+			
+			// 次の接続面を登録（コメントアウトすると頂点で接続された面を考慮しない）
+			if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
+				PfxInt32 nextEdgeId = (link.oedgeId+1)%3;
+				PfxMcEdge *nextEdge = ofacet.e[nextEdgeId];
+				if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i && 
+				  ((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 || 
+				   (PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) {
+					cqueue.push(PfxMcFacetLink(
+						link.baseEdgeId,
+						link.vid1,link.vid2,
+						link.ofacetId,link.iedgeId,
+						ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId]));
+				}
+				nextEdgeId = (link.oedgeId+2)%3;
+				nextEdge = ofacet.e[nextEdgeId];
+				if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i && 
+				  ((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 || 
+				   (PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) {
+					cqueue.push(PfxMcFacetLink(
+						link.baseEdgeId,
+						link.vid1,link.vid2,
+						link.ofacetId,link.iedgeId,
+						ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId]));
+				}
+			}
+		}
+	}
+	
+	// 面に厚みを付ける
+	if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
+		for(PfxUInt32 i=0;i<numTriangles;i++) {
+			PfxMcFacet &facetA = facetList[i];
+			for(PfxUInt32 j=0;j<numTriangles;j++) {
+				// 隣接面は比較対象にしない
+				if( i==j ||
+					j == (PfxInt32)facetA.e[0]->facetId[0] ||
+					j == (PfxInt32)facetA.e[0]->facetId[1] ||
+					j == (PfxInt32)facetA.e[1]->facetId[0] ||
+					j == (PfxInt32)facetA.e[1]->facetId[1] ||
+					j == (PfxInt32)facetA.e[2]->facetId[0] ||
+					j == (PfxInt32)facetA.e[2]->facetId[1]) {
+					continue;
+				}
+				
+				PfxMcFacet &facetB = facetList[j];
+				
+				// 交差判定
+				PfxFloat closestDistance=0;
+				if(intersect(facetA,facetB,closestDistance)) {
+					// 最近接距離/2を厚みとして採用
+					facetA.thickness = SCE_PFX_MAX(param.defaultThickness,SCE_PFX_MIN(facetA.thickness,closestDistance * 0.5f));
+				}
+			}
+
+		}
+	}
+
+	// 面の面積によって３種類に分類する
+	PfxFloat areaMin=SCE_PFX_FLT_MAX,areaMax=-SCE_PFX_FLT_MAX;
+	for(PfxUInt32 f=0;f<(PfxUInt32)numTriangles;f++) {
+		PfxVector3 pnts[3] = {
+			facetList[f].v[0]->coord,
+			facetList[f].v[1]->coord,
+			facetList[f].v[2]->coord,
+		};
+		areaMin = SCE_PFX_MIN(areaMin,facetList[f].area);
+		areaMax = SCE_PFX_MAX(areaMax,facetList[f].area);
+		
+		// 面のAABBを算出
+		facetList[f].aabbMin = minPerElem(pnts[2],minPerElem(pnts[1],pnts[0]));
+		facetList[f].aabbMax = maxPerElem(pnts[2],maxPerElem(pnts[1],pnts[0]));
+	}
+
+	PfxFloat areaDiff = (areaMax-areaMin)/3.0f;
+	PfxFloat areaLevel0,areaLevel1;
+	areaLevel0 = areaMin + areaDiff;
+	areaLevel1 = areaMin + areaDiff * 2.0f;
+
+	PfxArray<PfxMcFacetPtr> facetsLv0(numTriangles);
+	PfxArray<PfxMcFacetPtr> facetsLv1(numTriangles);
+	PfxArray<PfxMcFacetPtr> facetsLv2(numTriangles);
+
+	for(PfxUInt32 f=0;f<numTriangles;f++) {
+		PfxFloat area = facetList[f].area;
+
+		PfxMcFacet *fct = &facetList[f];
+		if(area < areaLevel0) {
+			facetsLv0.push(fct);
+		}
+		else if(area > areaLevel1) {
+			facetsLv2.push(fct);
+		}
+		else {
+			facetsLv1.push(fct);
+		}
+	}
+
+	// アイランドの配列
+	PfxMcIslands islands;
+	PfxVector3 lmeshSize;
+
+	// レベル毎にPfxTriMeshを作成
+	if(!facetsLv0.empty()) {
+		// 全体のAABBを求める
+		PfxVector3 aabbMin,aabbMax,center,half;
+		aabbMin =facetsLv0[0]->aabbMin;
+		aabbMax = facetsLv0[0]->aabbMax;
+		for(PfxUInt32 f=1;f<facetsLv0.size();f++) {
+			aabbMin = minPerElem(facetsLv0[f]->aabbMin,aabbMin);
+			aabbMax = maxPerElem(facetsLv0[f]->aabbMax,aabbMax);
+		}
+		center = ( aabbMin + aabbMax ) * 0.5f;
+		half = ( aabbMax - aabbMin ) * 0.5f;
+
+		// 再帰的に処理
+		divideMeshes(
+			param.numFacetsLimit,param.islandsRatio,
+			islands,
+			facetsLv0,
+			center,half);
+
+		lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
+	}
+
+	if(!facetsLv1.empty()) {
+		// 全体のAABBを求める
+		PfxVector3 aabbMin,aabbMax,center,half;
+		aabbMin =facetsLv1[0]->aabbMin;
+		aabbMax = facetsLv1[0]->aabbMax;
+		for(PfxUInt32 f=1;f<facetsLv1.size();f++) {
+			aabbMin = minPerElem(facetsLv1[f]->aabbMin,aabbMin);
+			aabbMax = maxPerElem(facetsLv1[f]->aabbMax,aabbMax);
+		}
+		center = ( aabbMin + aabbMax ) * 0.5f;
+		half = ( aabbMax - aabbMin ) * 0.5f;
+
+		// 再帰的に処理
+		divideMeshes(
+			param.numFacetsLimit,param.islandsRatio,
+			islands,
+			facetsLv1,
+			center,half);
+
+		lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
+	}
+
+	if(!facetsLv2.empty()) {
+		// 全体のAABBを求める
+		PfxVector3 aabbMin,aabbMax,center,half;
+		aabbMin =facetsLv2[0]->aabbMin;
+		aabbMax = facetsLv2[0]->aabbMax;
+		for(PfxUInt32 f=1;f<facetsLv2.size();f++) {
+			aabbMin = minPerElem(facetsLv2[f]->aabbMin,aabbMin);
+			aabbMax = maxPerElem(facetsLv2[f]->aabbMax,aabbMax);
+		}
+		center = ( aabbMin + aabbMax ) * 0.5f;
+		half = ( aabbMax - aabbMin ) * 0.5f;
+
+		// 再帰的に処理
+		divideMeshes(
+			param.numFacetsLimit,param.islandsRatio,
+			islands,
+			facetsLv2,
+			center,half);
+
+		lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
+	}
+
+	lmesh.m_half = lmeshSize;
+
+	// Check Islands
+	//for(PfxInt32 i=0;i<islands.numIslands;i++) {
+	//	SCE_PFX_PRINTF("island %d\n",i);
+	//	for(PfxInt32 f=0;f<islands.facetsInIsland[i].size();f++) {
+	//		PfxMcFacet *facet = islands.facetsInIsland[i][f];
+	//		SCE_PFX_PRINTF("   %d %d %d\n",facet->v[0]->i,facet->v[1]->i,facet->v[2]->i);
+	//	}
+	//}
+
+	// PfxLargeTriMeshの生成
+	if(islands.numIslands > 0) {
+		lmesh.m_numIslands = 0;
+		lmesh.m_aabbList = (PfxAabb16*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxAabb16)*islands.numIslands);
+		lmesh.m_islands = (PfxTriMesh*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxTriMesh)*islands.numIslands);
+		
+		PfxInt32 maxFacets=0,maxVerts=0,maxEdges=0;
+		for(PfxUInt32 i=0;i<islands.numIslands;i++) {
+			PfxTriMesh island;
+			createIsland(island,islands.facetsInIsland[i]);
+			addIslandToLargeTriMesh(lmesh,island);
+			maxFacets = SCE_PFX_MAX(maxFacets,island.m_numFacets);
+			maxVerts = SCE_PFX_MAX(maxVerts,island.m_numVerts);
+			maxEdges = SCE_PFX_MAX(maxEdges,island.m_numEdges);
+			//SCE_PFX_PRINTF("island %d verts %d edges %d facets %d\n",i,island.m_numVerts,island.m_numEdges,island.m_numFacets);
+		}
+
+		SCE_PFX_PRINTF("generate completed!\n\tinput mesh verts %d triangles %d\n\tislands %d max triangles %d verts %d edges %d\n",
+			param.numVerts,param.numTriangles,
+			lmesh.m_numIslands,maxFacets,maxVerts,maxEdges);
+		SCE_PFX_PRINTF("\tsizeof(PfxLargeTriMesh) %d sizeof(PfxTriMesh) %d\n",sizeof(PfxLargeTriMesh),sizeof(PfxTriMesh));
+	}
+	else {
+		SCE_PFX_PRINTF("islands overflow! %d/%d\n",islands.numIslands,SCE_PFX_LARGETRIMESH_MAX_ISLANDS);
+		return SCE_PFX_ERR_OUT_OF_RANGE;
+	}
+
+	return SCE_PFX_OK;
+}
+
+void pfxReleaseLargeTriMesh(PfxLargeTriMesh &lmesh)
+{
+	SCE_PFX_UTIL_FREE(lmesh.m_aabbList);
+	SCE_PFX_UTIL_FREE(lmesh.m_islands);
+	lmesh.m_numIslands = 0;
+}
+
+} //namespace PhysicsEffects
+} //namespace sce

Extras/PhysicsEffects/src/util/pfx_util_common.h

@@ -0,0 +1,32 @@
+/*
+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
+*/
+
+#ifndef _SCE_PFX_UTIL_COMMON_H
+#define _SCE_PFX_UTIL_COMMON_H
+
+#include <string.h>
+
+#ifdef _WIN32
+	#define SCE_PFX_UTIL_ALLOC(align,size) _aligned_malloc(size,align)
+	#define SCE_PFX_UTIL_REALLOC(ptr,align,size) _aligned_realloc(ptr,size,align)
+	#define SCE_PFX_UTIL_FREE(ptr) if(ptr) {_aligned_free(ptr);ptr=NULL;}
+#else
+	#define SCE_PFX_UTIL_ALLOC(align,size) malloc(size)
+        #define SCE_PFX_UTIL_REALLOC(ptr,align,size) realloc(ptr,size)
+        #define SCE_PFX_UTIL_FREE(ptr) if(ptr) {free(ptr);ptr=NULL;}
+#endif
+
+#endif // _SCE_PFX_UTIL_COMMON_H

Extras/PhysicsEffects/src/util/premake4.lua

@@ -0,0 +1,12 @@
+	project "physicseffects2_util"
+		
+	kind "StaticLib"
+	targetdir "../../build/lib"	
+	includedirs {
+		".",
+	}
+	files {
+		"**.cpp",
+		"../../include/physics_effects/util/**.h"
+
+	}