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/include/BulletPhysicsEffects/btBulletPhysicsEffectsWorld.cpp

@@ -0,0 +1,479 @@
+/*
+Physics Effects Copyright(C) 2011 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 "btBulletPhysicsEffectsWorld.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletMultiThreaded/SequentialThreadSupport.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btQuickprof.h"
+
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+
+
+#include "BulletPhysicsEffects/btLowLevelData.h"
+#include <physics_effects/low_level/pfx_low_level_include.h>
+#include <physics_effects/util/pfx_util_include.h>
+#include "btLowLevelBroadphase.h"
+
+
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"//required for shape conversion
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"//required for shape conversion
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"//required for shape conversion
+#include "BulletCollision/CollisionShapes/btSphereShape.h"//required for shape conversion
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"//required for shape conversion
+#include "BulletCollision/CollisionShapes/btBoxShape.h"//required for shape conversion
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletMultiThreaded/vectormath2bullet.h"
+
+#ifdef _WIN32
+#include "BulletMultiThreaded/Win32ThreadSupport.h"
+#endif
+
+
+#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
+#include "SpuDispatch/BulletPEGatherScatterSpursSupport.h"
+#include "SpuDispatch/SpuPEGatherScatterTaskProcess.h"
+#endif
+
+
+btBulletPhysicsEffectsWorld::btBulletPhysicsEffectsWorld(btLowLevelData* lowLevelData, btDispatcher* dispatcher,btLowLevelBroadphase* broadphase,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration, btThreadSupportInterface* threadSupport)
+:btDiscreteDynamicsWorld(dispatcher,broadphase,constraintSolver,collisionConfiguration),
+m_lowLevelData(lowLevelData)
+{
+
+#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
+	int numGatherScatterSpus = threadSupport->getNumTasks();
+	m_PEGatherScatterProcess = new SpuPEGatherScatterTaskProcess(threadSupport,numGatherScatterSpus);
+#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
+
+	m_lowLevelStates.resize(broadphase->m_maxHandles);//or use expandNonInitializing?
+	m_lowLevelBodies.resize(broadphase->m_maxHandles);
+	m_lowLevelSolverBodies.resize(broadphase->m_maxHandles);
+	m_lowLevelCollidables.resize(broadphase->m_maxHandles);
+
+	m_lowLevelData->m_states = &m_lowLevelStates[0];
+	m_lowLevelData->m_collidables = &m_lowLevelCollidables[0];
+	m_lowLevelData->m_bodies = &m_lowLevelBodies[0];
+	m_lowLevelData->m_solverBodies = &m_lowLevelSolverBodies[0];
+	m_lowLevelData->m_numRigidBodies = broadphase->m_maxHandles;
+
+}
+		
+btBulletPhysicsEffectsWorld::~btBulletPhysicsEffectsWorld()
+{
+#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
+	delete	m_PEGatherScatterProcess;
+#endif
+
+}
+
+
+
+void	btBulletPhysicsEffectsWorld::integrateTransforms(btScalar timeStep)
+{
+			///integrate transforms
+#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
+	if (getDispatchInfo().m_enableSPU)
+	{
+		BT_PROFILE("integrateTransformsSPU");
+		int numRemainingObjects = m_nonStaticRigidBodies.size();
+
+		int batchSize = PARALLEL_BATCH_SIZE;
+		int startIndex = 0;
+		while (numRemainingObjects>0)
+		{
+			int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;
+
+			//issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)
+			m_PEGatherScatterProcess->issueTask(
+													CMD_SAMPLE_INTEGRATE_BODIES,
+													&m_nonStaticRigidBodies[0],
+													0,
+													0,
+													startIndex,
+													currentBatchSize);
+			numRemainingObjects -= currentBatchSize;
+			startIndex += currentBatchSize;
+		}
+		
+		m_PEGatherScatterProcess->flush();
+	} else
+#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
+	{
+//		BT_PROFILE("integrateTransforms");
+		btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+	}
+}
+
+
+void	btBulletPhysicsEffectsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
+	if (getDispatchInfo().m_enableSPU)
+	{
+		BT_PROFILE("predictUnconstraintMotionSPU");
+		int numRemainingObjects = m_nonStaticRigidBodies.size();
+
+		int batchSize = PARALLEL_BATCH_SIZE;
+		int startIndex=0;
+		while (numRemainingObjects>0)
+		{
+			int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;
+
+			//issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)
+			m_PEGatherScatterProcess->issueTask(
+													CMD_SAMPLE_PREDICT_MOTION_BODIES,
+													&m_nonStaticRigidBodies[0],
+													0,
+													0,
+													startIndex,
+													currentBatchSize);
+			numRemainingObjects -= currentBatchSize;
+			startIndex += currentBatchSize;
+		}
+		
+		m_PEGatherScatterProcess->flush();
+	}
+	else
+#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
+	{
+		btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
+	}
+
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		body->setHitFraction(1.f);
+
+		if (body->isActive() && (!body->isStaticOrKinematicObject()))
+		{
+			syncRigidBodyState(body);
+		}
+	}
+}
+
+
+
+void	btBulletPhysicsEffectsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+{
+	BT_PROFILE("solveConstraints");
+
+	btCollisionDispatcher* disp = (btCollisionDispatcher*) getDispatcher();
+	int numBodies = getNumCollisionObjects();
+	
+	btPersistentManifold** manifolds = disp->getInternalManifoldPointer();
+	
+	int numManifolds = disp->getNumManifolds();
+	
+	
+	if ((getNumCollisionObjects()>0) && (numManifolds + m_constraints.size()>0))
+	{
+
+		btCollisionObject** bodies = numBodies ? &getCollisionObjectArray()[0] : 0;
+		btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
+
+		getConstraintSolver()->solveGroup( bodies,numBodies, disp->getInternalManifoldPointer(),numManifolds, constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,disp);
+	}
+}
+
+
+void	btBulletPhysicsEffectsWorld::calculateSimulationIslands()
+{
+
+}
+
+static void	convertShape(btCollisionShape* bulletShape, btAlignedObjectArray<sce::PhysicsEffects::PfxShape>& shapes)
+{
+	switch (bulletShape->getShapeType())
+	{
+	case BOX_SHAPE_PROXYTYPE:
+		{
+			btBoxShape* boxshape = (btBoxShape*)bulletShape;
+			sce::PhysicsEffects::PfxBox box(boxshape->getHalfExtentsWithMargin().getX(),boxshape->getHalfExtentsWithMargin().getY(),boxshape->getHalfExtentsWithMargin().getZ());
+			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
+			shape.reset();
+			shape.setBox(box);
+			break;
+		}
+
+	case TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) bulletShape;
+
+			int numSubParts = trimesh->getMeshInterface()->getNumSubParts();
+			btAssert(numSubParts>0);
+			
+			for (int i=0;i<numSubParts;i++)
+			{
+				unsigned char* vertexBase=0;
+				int numVerts = 0;
+				PHY_ScalarType vertexType;
+				int vertexStride=0;
+				unsigned char* indexBase=0;
+				int indexStride=0;
+				int numFaces=0;
+				PHY_ScalarType indexType;
+
+				trimesh->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numVerts,vertexType,vertexStride,&indexBase,indexStride,numFaces,indexType,i);
+
+				sce::PhysicsEffects::PfxCreateLargeTriMeshParam param;
+				btAssert(param.flag&SCE_PFX_MESH_FLAG_16BIT_INDEX);
+				unsigned short int* copyIndices = new unsigned short int[numFaces*3];
+				switch (indexType)
+				{
+				case PHY_UCHAR:	
+					{
+						for (int p=0;p<numFaces;p++)
+						{
+							copyIndices[p*3]=indexBase[p*indexStride];
+							copyIndices[p*3+1]=indexBase[p*indexStride+1];
+							copyIndices[p*3+2]=indexBase[p*indexStride+2];
+						}
+						break;
+					}
+					//PHY_INTEGER:
+					//PHY_SHORT:
+				default:
+					{
+						btAssert(0);
+					}
+				};
+				
+				param.verts = (float*)vertexBase;
+				param.numVerts = numVerts;
+				param.vertexStrideBytes = vertexStride;
+
+				param.triangles = copyIndices;
+				param.numTriangles = numFaces;
+				param.triangleStrideBytes = sizeof(unsigned short int)*3;
+				
+				sce::PhysicsEffects::PfxLargeTriMesh* largeMesh = new sce::PhysicsEffects::PfxLargeTriMesh();
+
+				sce::PhysicsEffects::PfxInt32 ret = pfxCreateLargeTriMesh(*largeMesh,param);
+				if(ret != SCE_PFX_OK) {
+					SCE_PFX_PRINTF("Can't create large mesh.\n");
+				}
+
+				sce::PhysicsEffects::PfxShape& shape = shapes.expand();
+				shape.reset();
+				shape.setLargeTriMesh(largeMesh);
+			}
+
+			break;
+		}
+	case SPHERE_SHAPE_PROXYTYPE:
+		{
+			btSphereShape* sphereshape = (btSphereShape*)bulletShape;
+			sce::PhysicsEffects::PfxSphere sphere(sphereshape->getRadius());
+			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
+			shape.reset();
+			shape.setSphere(sphere);
+			break;
+		}
+	case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			btCapsuleShape* capsuleshape= (btCapsuleShape*)bulletShape;//assume btCapsuleShapeX for now
+			sce::PhysicsEffects::PfxCapsule capsule(capsuleshape->getHalfHeight(),capsuleshape->getRadius());
+			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
+			shape.reset();
+			shape.setCapsule(capsule);
+			break;
+		}
+	case CYLINDER_SHAPE_PROXYTYPE:
+		{
+			btCylinderShape* cylindershape= (btCylinderShape*)bulletShape;//assume btCylinderShapeX for now
+			sce::PhysicsEffects::PfxCylinder cylinder(cylindershape->getHalfExtentsWithMargin()[0],cylindershape->getRadius());
+			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
+			shape.reset();
+			shape.setCylinder(cylinder);
+			break;
+		}
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			btConvexHullShape* convexHullShape = (btConvexHullShape*)bulletShape;
+			
+			sce::PhysicsEffects::PfxConvexMesh* convex = new sce::PhysicsEffects::PfxConvexMesh();
+			convex->m_numVerts = convexHullShape->getNumPoints();
+			convex->m_numIndices = 0;//todo for ray intersection test support
+			
+			for (int i=0;i<convex->m_numVerts;i++)
+			{
+				convex->m_verts[i].setX(convexHullShape->getPoints()[i].getX());
+				convex->m_verts[i].setY(convexHullShape->getPoints()[i].getY());
+				convex->m_verts[i].setZ(convexHullShape->getPoints()[i].getZ());
+			}
+
+			convex->updateAABB();
+			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
+			shape.reset();
+			shape.setConvexMesh(convex);
+			break;
+		}
+	case COMPOUND_SHAPE_PROXYTYPE:
+		{
+			btCompoundShape* compound = (btCompoundShape*) bulletShape;
+			
+			for (int s=0;s<compound->getNumChildShapes();s++)
+			{
+				convertShape(compound->getChildShape(s),shapes);
+
+				sce::PhysicsEffects::PfxMatrix3 rotMat = getVmMatrix3(compound->getChildTransform(s).getBasis());
+				sce::PhysicsEffects::PfxVector3 translate = getVmVector3(compound->getChildTransform(s).getOrigin());
+				sce::PhysicsEffects::PfxTransform3 childtransform(rotMat,translate);
+				shapes[shapes.size()-1].setOffsetTransform(childtransform);
+			}
+
+			break;
+		}
+
+
+	default:
+		{
+			btAssert(0);
+		}
+	};
+
+}
+
+void	btBulletPhysicsEffectsWorld::createStateAndCollidable(btRigidBody* body)
+{
+	
+	int objectIndex = body->getBroadphaseProxy()->m_uniqueId;
+	btAssert(objectIndex>=0);
+	//btAssert(objectIndex<m_maxHandles);
+
+	//initialize it
+	sce::PhysicsEffects::PfxRigidBody* pfxbody = &m_lowLevelBodies[objectIndex];
+	sce::PhysicsEffects::PfxRigidState* pfxstate = &m_lowLevelStates[objectIndex];
+	sce::PhysicsEffects::PfxCollidable* pfxcollidable = &m_lowLevelCollidables[objectIndex];
+
+	///convert/initialize body, shape, state and collidable
+	pfxstate->reset();
+	pfxbody->reset();
+	pfxcollidable->reset();
+	
+	
+	
+	pfxbody->setFriction(body->getFriction());
+	pfxbody->setRestitution(body->getRestitution());
+
+	if (body->getInvMass())
+	{
+		btScalar mass = 1.f/body->getInvMass();
+		pfxbody->setMass(mass);
+		Vectormath::Aos::Matrix3 inertiaInv = inertiaInv.identity();
+		inertiaInv.setElem(0,0,body->getInvInertiaDiagLocal().getX());
+		inertiaInv.setElem(1,1,body->getInvInertiaDiagLocal().getY());
+		inertiaInv.setElem(2,2,body->getInvInertiaDiagLocal().getZ());
+		pfxbody->setInertiaInv(inertiaInv);
+		pfxstate->setMotionType(sce::PhysicsEffects::kPfxMotionTypeActive);
+	} else
+	{
+		pfxstate->setMotionType(sce::PhysicsEffects::kPfxMotionTypeFixed);
+	}
+
+	btAlignedObjectArray<sce::PhysicsEffects::PfxShape> shapes;
+	
+	convertShape(body->getCollisionShape(), shapes);
+
+	btAssert(shapes.size()>0);
+
+	if (shapes.size()==1)
+	{
+		pfxcollidable->addShape(shapes[0]);
+		pfxcollidable->finish();
+	} else
+	{
+		if (shapes.size()>1)
+		{
+			sce::PhysicsEffects::PfxUInt16* ints=new sce::PhysicsEffects::PfxUInt16[shapes.size()];
+			sce::PhysicsEffects::PfxShape* pfxshapes = new sce::PhysicsEffects::PfxShape[shapes.size()];
+			int p;
+			for (p=0;p<shapes.size();p++)
+			{
+				ints[p]=p;
+				pfxshapes[p] = shapes[p];
+			}
+			pfxcollidable->reset(pfxshapes,ints,shapes.size());
+			
+			for (p=0;p<shapes.size();p++)
+			{
+				pfxcollidable->addShape(pfxshapes[p]);
+			}
+			
+			pfxcollidable->finish();
+		}
+	}
+	pfxstate->setRigidBodyId(objectIndex);
+
+	syncRigidBodyState(body);
+		
+	
+}
+
+
+
+void	btBulletPhysicsEffectsWorld::syncRigidBodyState(btRigidBody* body)
+{
+	int objectIndex = body->getBroadphaseProxy()->m_uniqueId;
+	sce::PhysicsEffects::PfxRigidState* pfxstate = &m_lowLevelStates[objectIndex];
+
+	pfxstate->setPosition(sce::PhysicsEffects::PfxVector3(body->getWorldTransform().getOrigin()[0],body->getWorldTransform().getOrigin()[1],body->getWorldTransform().getOrigin()[2]));
+	sce::PhysicsEffects::PfxQuat rot(body->getOrientation().getX(),body->getOrientation().getY(),body->getOrientation().getZ(),body->getOrientation().getW());
+	pfxstate->setOrientation(rot);
+
+}
+	
+
+
+void	btBulletPhysicsEffectsWorld::addRigidBody(btRigidBody* body)
+{
+
+	btDiscreteDynamicsWorld::addRigidBody(body);
+
+	//create a state and collidable
+		
+	createStateAndCollidable(body);
+
+
+	
+
+
+//	m_lowLevelData->m_numRigidBodies++;
+//	btAssert(m_lowLevelData->m_numRigidBodies<m_lowLevelData->m_maxNumRigidBodies);
+	
+}
+
+void	btBulletPhysicsEffectsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+{
+	btDiscreteDynamicsWorld::addRigidBody(body,group,mask);
+
+}
+
+void	btBulletPhysicsEffectsWorld::removeRigidBody(btRigidBody* body)
+{
+	btDiscreteDynamicsWorld::removeRigidBody(body);
+
+//	m_lowLevelData->m_numRigidBodies--;
+//	btAssert(m_lowLevelData->m_numRigidBodies>=0);
+}
+

Extras/PhysicsEffects/include/BulletPhysicsEffects/btBulletPhysicsEffectsWorld.h

@@ -0,0 +1,79 @@
+/*
+Physics Effects Copyright(C) 2011 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 _BT_PARALLEL_DYNAMICS_WORLD_H
+#define _BT_PARALLEL_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "physics_effects/base_level/rigidbody/pfx_rigid_body.h"
+#include "physics_effects/base_level/rigidbody/pfx_rigid_state.h"
+#include "physics_effects/base_level/collision/pfx_collidable.h"
+#include "physics_effects/base_level/solver/pfx_solver_body.h"
+
+//#define USE_PE_GATHER_SCATTER_SPURS_TASK 1
+
+
+class btThreadSupportInterface;
+class SpuPEGatherScatterTaskProcess;
+struct CellSpurs;
+class SpuBatchRaycaster;
+struct btLowLevelBroadphase;
+
+#define PARALLEL_BATCH_SIZE 64
+
+
+///btBulletPhysicsEffectsWorld adds parallel processing for integration/motion prediction
+class btBulletPhysicsEffectsWorld : public btDiscreteDynamicsWorld
+{
+
+protected:
+	btAlignedObjectArray<sce::PhysicsEffects::PfxRigidState>	m_lowLevelStates;
+	btAlignedObjectArray<sce::PhysicsEffects::PfxRigidBody>		m_lowLevelBodies;
+	btAlignedObjectArray<sce::PhysicsEffects::PfxSolverBody>	m_lowLevelSolverBodies;
+	btAlignedObjectArray<sce::PhysicsEffects::PfxCollidable>	m_lowLevelCollidables;
+
+	//PfxSolverBody solverBodies[NUM_RIGIDBODIES];
+
+	class SpuPEGatherScatterTaskProcess*	m_PEGatherScatterProcess;
+
+	struct btLowLevelData*							m_lowLevelData;
+
+	void	createStateAndCollidable(btRigidBody* body);
+
+	void	syncRigidBodyState(btRigidBody* body);
+public:
+
+	btBulletPhysicsEffectsWorld(struct btLowLevelData* lowLevelData, btDispatcher* dispatcher,btLowLevelBroadphase* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration, btThreadSupportInterface* threadSupport);
+		
+	virtual ~btBulletPhysicsEffectsWorld();
+	
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
+		
+	virtual void integrateTransforms(btScalar timeStep);
+
+	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+
+	virtual void	calculateSimulationIslands();
+
+	virtual void	addRigidBody(btRigidBody* body);
+
+	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+
+	virtual void	removeRigidBody(btRigidBody* body);
+
+
+};
+#endif //_BT_PARALLEL_DYNAMICS_WORLD_H

Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelBroadphase.cpp

@@ -0,0 +1,431 @@
+/*
+Physics Effects Copyright(C) 2011 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 "btLowLevelBroadphase.h"
+
+
+#include "BulletMultiThreaded/PlatformDefinitions.h"
+#include "BulletCollision/NarrowphaseCollision/btPersistentManifold.h"
+
+// Include base level headers
+#include "physics_effects/base_level/pfx_base_level_include.h"
+
+// Include low level headers
+#include "physics_effects/low_level/broadphase/pfx_broadphase.h"
+
+#include "physics_effects/low_level/sort/pfx_parallel_sort.h"
+
+#include "BulletMultiThreaded/vectormath2bullet.h"
+
+#include "physics_effects/base_level/base/pfx_vec_utils.h"
+#include "physics_effects/base_level/collision/pfx_aabb.h"
+#include "physics_effects/base_level/rigidbody/pfx_rigid_state.h"
+#include "btLowLevelData.h"
+
+using namespace sce::PhysicsEffects;
+
+//E Temporary buffers
+#define POOL_BYTES (5*1024*1024)
+unsigned char SCE_PFX_ALIGNED(128) poolBuff[POOL_BYTES];
+
+//E Stack allocator for temporary buffers
+PfxHeapManager pool(poolBuff,POOL_BYTES);
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Broadphase
+
+
+
+
+
+//
+btLowLevelBroadphase::btLowLevelBroadphase(btLowLevelData* lowLevelData, btOverlappingPairCache* paircache, int maxProxies)
+:m_lowLevelData(lowLevelData)
+{
+	
+	m_guidGenerator = 1;
+	m_releasepaircache	=	(paircache!=0)?false:true;
+	m_paircache			=	paircache? paircache	: new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+	m_clientData.m_bp = this;
+	m_clientData.m_dispatcher = 0;
+
+
+	m_broadphaseAabbMin.setValue(1e30,1e30,1e30);
+	m_broadphaseAabbMax.setValue(-1e30,-1e30,-1e30);
+
+		// allocate handles buffer and put all handles on free list
+	m_pHandlesRawPtr = btAlignedAlloc(sizeof(btLowLevelBroadphaseProxy)*maxProxies,16);
+	m_pHandles = new(m_pHandlesRawPtr) btLowLevelBroadphaseProxy[maxProxies];
+	m_maxHandles = maxProxies;
+	m_numHandles = 0;
+	m_firstFreeHandle = 0;
+	m_LastHandleIndex = -1;
+	
+
+	{
+		for (int i = m_firstFreeHandle; i < maxProxies; i++)
+		{
+			m_pHandles[i].SetNextFree(i + 1);
+			m_pHandles[i].m_uniqueId = i;//start from zero, so we can re-use the uid for body ID
+		}
+		m_pHandles[maxProxies - 1].SetNextFree(0);
+	
+	}
+
+}
+
+//
+btLowLevelBroadphase::~btLowLevelBroadphase()
+{
+	if(m_releasepaircache) 
+	{
+		m_paircache->~btOverlappingPairCache();
+		btAlignedFree(m_paircache);
+	}
+}
+
+
+btBroadphaseProxy*	btLowLevelBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
+{
+	if (m_numHandles >= m_maxHandles)
+	{
+		btAssert(0);
+		return 0; //should never happen, but don't let the game crash ;-)
+	}
+	btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
+
+	int newHandleIndex = allocHandle();
+	btLowLevelBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btLowLevelBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
+	m_uid2ptr.insert(proxy->m_uniqueId,proxy);
+	return proxy;
+}
+
+void	btLowLevelBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
+{
+		m_uid2ptr.remove(proxyOrg->m_uniqueId);
+
+		btLowLevelBroadphaseProxy* proxy0 = static_cast<btLowLevelBroadphaseProxy*>(proxyOrg);
+		freeHandle(proxy0);
+
+		m_paircache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
+
+		//validate();
+		
+}
+
+void	btLowLevelBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	const btLowLevelBroadphaseProxy* sbp = getLowLevelProxyFromProxy(proxy);
+	aabbMin = sbp->m_aabbMin;
+	aabbMax = sbp->m_aabbMax;
+}
+
+void	btLowLevelBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
+{
+	btLowLevelBroadphaseProxy* sbp = getLowLevelProxyFromProxy(proxy);
+	sbp->m_aabbMin = aabbMin;
+	sbp->m_aabbMax = aabbMax;
+}
+
+
+bool	btLowLevelBroadphase::aabbOverlap(btLowLevelBroadphaseProxy* proxy0,btLowLevelBroadphaseProxy* proxy1)
+{
+	return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && 
+		   proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
+		   proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
+
+}
+
+
+PfxBroadphasePair*		btLowLevelBroadphase::getCurrentPairs()
+{
+	return &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];
+}
+
+const PfxBroadphasePair*		btLowLevelBroadphase::getCurrentPairs() const
+{
+	return &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];
+}
+
+int	btLowLevelBroadphase::getNumCurrentPairs() const
+{
+	return m_lowLevelData->m_numPairs[m_lowLevelData->m_pairSwap];
+}
+
+void btLowLevelBroadphase::broadphase(PfxSortData32* proxies, int numRigidBodies, int axis, btDispatcher* dispatcher)
+{
+	m_lowLevelData->m_pairSwap = 1-m_lowLevelData->m_pairSwap;
+
+	unsigned int &numPreviousPairs = m_lowLevelData->m_numPairs[1-m_lowLevelData->m_pairSwap];
+	unsigned int &numCurrentPairs = m_lowLevelData->m_numPairs[m_lowLevelData->m_pairSwap];
+	PfxBroadphasePair *previousPairs = &m_lowLevelData->m_pairsBuff[1-m_lowLevelData->m_pairSwap][0];
+	PfxBroadphasePair *currentPairs = &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];
+
+
+	//E Create broadpahse proxies
+	{
+//		for(int i=0;i<numRigidBodies;i++) {
+//			pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis);
+//		}
+
+		int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies;
+		void *workBuff = pool.allocate(workBytes);
+				
+		pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes);
+
+		pool.deallocate(workBuff);
+	}
+
+	//E Find overlapped pairs
+	{
+		PfxFindPairsParam findPairsParam;
+		findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(m_lowLevelData->m_maxPairs);
+		findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes);
+		findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(m_lowLevelData->m_maxPairs);
+		findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes);
+		findPairsParam.proxies = proxies;
+		findPairsParam.numProxies = numRigidBodies;
+		findPairsParam.maxPairs = m_lowLevelData->m_maxPairs;
+		findPairsParam.axis = axis;
+
+		PfxFindPairsResult findPairsResult;
+
+
+		int ret = pfxFindPairs(findPairsParam,findPairsResult);
+		if(ret != SCE_PFX_OK) 
+			SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);
+		
+		pool.deallocate(findPairsParam.workBuff);
+
+		//E Decompose overlapped pairs into 3 arrays
+		PfxDecomposePairsParam decomposePairsParam;
+		decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
+		decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes);
+		decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
+		decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes);
+		decomposePairsParam.previousPairs = previousPairs;
+		decomposePairsParam.numPreviousPairs = numPreviousPairs;
+		decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs()
+		decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs()
+
+		PfxDecomposePairsResult decomposePairsResult;
+
+		ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);
+		if(ret != SCE_PFX_OK) 
+			SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);
+
+		pool.deallocate(decomposePairsParam.workBuff);
+
+		PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs;
+		PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs;
+		PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs;
+		PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs;
+		PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs;
+		PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs;
+
+		for (int i=0;i<numOutRemovePairs;i++)
+		{
+			int idA = pfxGetObjectIdA(outRemovePairs[i]);
+			int idB = pfxGetObjectIdB(outRemovePairs[i]);
+			//use m_uid2ptr to get pointer
+			
+			btBroadphaseProxy* proxyA = (btBroadphaseProxy*)*m_uid2ptr[idA];
+			btBroadphaseProxy* proxyB = (btBroadphaseProxy*)*m_uid2ptr[idB];
+			m_paircache->removeOverlappingPair(proxyA,proxyB,dispatcher);
+
+			//free low level contacts
+			m_lowLevelData->m_contactIdPool[m_lowLevelData->m_numContactIdPool++] = pfxGetContactId(outRemovePairs[i]);
+
+		}
+
+		for (int i=0;i<numOutNewPairs;i++)
+		{
+			int idA = pfxGetObjectIdA(outNewPairs[i]);
+			int idB = pfxGetObjectIdB(outNewPairs[i]);
+			//use m_uid2ptr to get pointer
+			
+			btBroadphaseProxy* proxyA = (btBroadphaseProxy*)*m_uid2ptr[idA];
+			btBroadphaseProxy* proxyB = (btBroadphaseProxy*)*m_uid2ptr[idB];
+			btBroadphasePair* btpair = m_paircache->addOverlappingPair(proxyA,proxyB);
+
+			//initialize low level contacts
+			int cId = 0;
+			if(m_lowLevelData->m_numContactIdPool > 0) {
+				cId = m_lowLevelData->m_contactIdPool[--m_lowLevelData->m_numContactIdPool];
+			}
+			else {
+				cId = m_lowLevelData->m_numContacts++;
+			}
+			if(cId >= m_lowLevelData->m_maxContacts) {
+				cId = 0;
+			}
+			SCE_PFX_ASSERT(cId < m_lowLevelData->m_maxContacts);
+			pfxSetContactId(outNewPairs[i],cId);
+			PfxContactManifold &contact = m_lowLevelData->m_contacts[cId];
+			int sz = sizeof(PfxContactManifold);
+			int sz2 = sizeof(btPersistentManifold);
+			int sz3 = 4*3*sizeof(btConstraintRow);
+			contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i]));
+			contact.setCompositeFriction(0.1f);
+			btpair->m_internalTmpValue = cId;
+		}
+		
+
+		//E Merge 'new' and 'keep' pairs
+		numCurrentPairs = 0;
+		for(PfxUInt32 i=0;i<numOutKeepPairs;i++) {
+			currentPairs[numCurrentPairs++] = outKeepPairs[i];
+		}
+		for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
+			currentPairs[numCurrentPairs++] = outNewPairs[i];
+		}
+		
+		pool.deallocate(decomposePairsParam.pairBuff);
+		pool.deallocate(findPairsParam.pairBuff);
+	}
+	
+	{
+		int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;
+		void *workBuff = pool.allocate(workBytes);
+		
+		pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);
+		
+		pool.deallocate(workBuff);
+	}
+}
+
+
+PfxInt32 MyUpdateBroadphaseProxy(PfxBroadphaseProxy& proxy,int rigidbodyId,const btBroadphaseProxy* bulletProxy, const PfxVector3& worldCenter,const PfxVector3& worldExtent,PfxUInt32 axis)
+{
+	SCE_PFX_ALWAYS_ASSERT(axis<3);
+	
+	PfxInt32 ret = SCE_PFX_OK;
+	
+	PfxVector3 minRig = getVmVector3(bulletProxy->m_aabbMin);
+	PfxVector3 maxRig = getVmVector3(bulletProxy->m_aabbMax);
+	
+	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,rigidbodyId);
+	pfxSetMotionMask(proxy, kPfxMotionTypeActive);
+	pfxSetSelf(proxy,bulletProxy->m_collisionFilterGroup);
+	pfxSetTarget(proxy,bulletProxy->m_collisionFilterMask);
+	
+	return ret;
+}
+
+void	btLowLevelBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+
+	//set the broadphase proxies
+
+	btAlignedObjectArray<PfxBroadphaseProxy> proxies;
+	proxies.reserve(m_LastHandleIndex);
+
+	//E Find the axis along which all rigid bodies are most widely positioned
+	int axis = 0;
+	int i;
+	PfxVector3 s(0.0f),s2(0.0f);
+
+	PfxVector3 worldMin(-1000);//PFX_FLT_MAX);
+	PfxVector3 worldMax(1000);//-PFX_FLT_MAX);
+	
+	int numRigidBodies = 0;
+	for (i=0; i <= m_LastHandleIndex; i++)
+	{
+		btLowLevelBroadphaseProxy* proxy0 = &m_pHandles[i];
+		if(!proxy0->m_clientObject)
+		{
+			continue;
+		}
+		
+		PfxVector3 pe_pos = getVmVector3(0.5f*(proxy0->m_aabbMax+proxy0->m_aabbMin));
+		PfxVector3 pe_min = getVmVector3(proxy0->m_aabbMin);
+		PfxVector3 pe_max = getVmVector3(proxy0->m_aabbMax);
+		numRigidBodies++;
+		//worldMin = minPerElem(worldMin,pe_min);
+		//worldMax = maxPerElem(worldMax,pe_max);
+
+		s += pe_pos;
+		s2 += mulPerElem(pe_pos,pe_pos);
+	}
+
+	
+	if (numRigidBodies)
+	{
+		PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;
+		if(v[1] > v[0]) 
+			axis = 1;
+		if(v[2] > v[axis]) 
+			axis = 2;
+	}
+
+	PfxVector3 worldCenter = 0.5f*(worldMax+worldMin);
+	PfxVector3 worldExtent = 0.5f*(worldMax-worldMin);
+
+	for (i=0; i <= m_LastHandleIndex; i++)
+	{
+		btLowLevelBroadphaseProxy* proxy0 = &m_pHandles[i];
+		if(!proxy0->m_clientObject)
+		{
+			continue;
+		}
+
+		PfxBroadphaseProxy& proxy = proxies.expandNonInitializing();
+		MyUpdateBroadphaseProxy(proxy,proxy0->m_uniqueId,proxy0,worldCenter,worldExtent,axis);
+
+	}
+
+	//find pairs, and call 'addOverlappingPair' for new pairs and 'removeOverlappingPair' for removed pairs
+	broadphase(&proxies[0],proxies.size(),axis, dispatcher);
+}
+
+
+//
+btOverlappingPairCache*			btLowLevelBroadphase::getOverlappingPairCache()
+{
+	return(m_paircache);
+}
+
+//
+const btOverlappingPairCache*	btLowLevelBroadphase::getOverlappingPairCache() const
+{
+	return(m_paircache);
+}
+
+
+void							btLowLevelBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+{
+	aabbMin = m_broadphaseAabbMin;
+	aabbMax = m_broadphaseAabbMax;
+}
+
+void	btLowLevelBroadphase::printStats()
+{
+}
+
+void	btLowLevelBroadphase::setNumTasks(int numTasks)
+{
+}

Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelBroadphase.h

@@ -0,0 +1,182 @@
+/*
+Physics Effects Copyright(C) 2011 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
+*/
+ 
+///btLowLevelBroadphase implementation
+#ifndef BT_LOW_LEVEL_BROADPHASE_H
+#define BT_LOW_LEVEL_BROADPHASE_H
+
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+
+#include "physics_effects/base_level/broadphase/pfx_broadphase_pair.h"
+#include "LinearMath/btHashMap.h"
+
+struct btLowLevelBroadphase;
+struct btLowLevelData;
+
+namespace sce
+{
+	namespace PhysicsEffects
+	{
+		struct PfxSortData32;
+	};
+};
+
+struct	btMyClientData
+{
+	btDispatcher* m_dispatcher;
+	btLowLevelBroadphase*	m_bp;
+};
+
+struct btLowLevelBroadphaseProxy : public btBroadphaseProxy
+{
+	int			m_nextFree;
+	
+//	int			m_handleId;
+
+	
+	btLowLevelBroadphaseProxy() {};
+
+	btLowLevelBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
+	:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
+	{
+		(void)shapeType;
+	}
+	
+	
+	SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
+	SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}
+
+	
+
+
+};
+
+///btLowLevelBroadphase is a binding between Open Physics low-level broadphase and Bullet, through the btBroadphaseInterface
+
+struct	btLowLevelBroadphase : btBroadphaseInterface
+{
+	
+		int		m_numHandles;						// number of active handles
+	int		m_maxHandles;						// max number of handles
+	int		m_LastHandleIndex;							
+	
+	btLowLevelBroadphaseProxy* m_pHandles;						// handles pool
+
+	void* m_pHandlesRawPtr;
+	int		m_firstFreeHandle;		// free handles list
+	
+	btOverlappingPairCache*	m_paircache;				// Pair cache
+	bool					m_releasepaircache;			// Release pair cache on delete
+	int						m_guidGenerator;
+	btVector3				m_broadphaseAabbMin;
+	btVector3				m_broadphaseAabbMax;
+
+	btMyClientData			m_clientData;
+
+	btLowLevelData*	m_lowLevelData;
+	btHashMap<btHashInt,void*> m_uid2ptr;
+
+
+	int allocHandle()
+	{
+		btAssert(m_numHandles < m_maxHandles);
+		int freeHandle = m_firstFreeHandle;
+		m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
+		m_numHandles++;
+		if(freeHandle > m_LastHandleIndex)
+		{
+			m_LastHandleIndex = freeHandle;
+		}
+		return freeHandle;
+	}
+
+	void freeHandle(btLowLevelBroadphaseProxy* proxy)
+	{
+		int handle = int(proxy-m_pHandles);
+		btAssert(handle >= 0 && handle < m_maxHandles);
+		if(handle == m_LastHandleIndex)
+		{
+			m_LastHandleIndex--;
+		}
+		proxy->SetNextFree(m_firstFreeHandle);
+		m_firstFreeHandle = handle;
+
+		proxy->m_clientObject = 0;
+
+		m_numHandles--;
+	}
+
+	inline btLowLevelBroadphaseProxy*	getLowLevelProxyFromProxy(btBroadphaseProxy* proxy)
+	{
+		btLowLevelBroadphaseProxy* proxy0 = static_cast<btLowLevelBroadphaseProxy*>(proxy);
+		return proxy0;
+	}
+
+	inline const btLowLevelBroadphaseProxy*	getLowLevelProxyFromProxy(btBroadphaseProxy* proxy) const
+	{
+		const btLowLevelBroadphaseProxy* proxy0 = static_cast<const btLowLevelBroadphaseProxy*>(proxy);
+		return proxy0;
+	}
+
+
+
+	inline bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		btLowLevelBroadphaseProxy* p0 = getLowLevelProxyFromProxy(proxy0);
+		btLowLevelBroadphaseProxy* p1 = getLowLevelProxyFromProxy(proxy1);
+		return aabbOverlap(p0,p1);
+	}
+
+	static bool	aabbOverlap(btLowLevelBroadphaseProxy* proxy0,btLowLevelBroadphaseProxy* proxy1);
+
+	void broadphase(sce::PhysicsEffects::PfxSortData32* proxies, int numRigidBodies, int axis, btDispatcher* dispatcher);
+
+	sce::PhysicsEffects::PfxBroadphasePair*		getCurrentPairs();
+	const sce::PhysicsEffects::PfxBroadphasePair*		getCurrentPairs() const;
+	int	getNumCurrentPairs() const;
+
+	
+
+	/* Methods		*/ 
+	btLowLevelBroadphase(btLowLevelData* lowLevelData, btOverlappingPairCache* paircache, int maxProxies = 16384);//,class PfxAllocator* allocator,class PfxStackAllocator* stackPool,void* spursInstance);
+	virtual ~btLowLevelBroadphase();
+
+	/* btBroadphaseInterface Implementation	*/ 
+	btBroadphaseProxy*				createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+	void							destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	void							setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0))
+	{
+	}
+	
+	virtual void	aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
+	{
+	}
+
+
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+	void							calculateOverlappingPairs(btDispatcher* dispatcher);
+	btOverlappingPairCache*			getOverlappingPairCache();
+	const btOverlappingPairCache*	getOverlappingPairCache() const;
+	void							getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void	printStats();
+
+	virtual void	setNumTasks(int numTasks);
+	
+};
+
+#endif

Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelCollisionDispatcher.h

@@ -0,0 +1,127 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_LOW_LEVEL_COLLISION__DISPATCHER_H
+#define BT_LOW_LEVEL_COLLISION__DISPATCHER_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+struct btLowLevelBroadphase;//struct instead of class?
+struct btLowLevelData;
+///Tuning value to optimized SPU utilization 
+///Too small value means Task overhead is large compared to computation (too fine granularity)
+///Too big value might render some SPUs are idle, while a few other SPUs are doing all work.
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 8
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 64
+#define SPU_BATCHSIZE_BROADPHASE_PAIRS 128
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 512
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 1024
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+class btLowLevelCollisionAlgorithm : public btCollisionAlgorithm
+{
+	btPersistentManifold* m_manifold;
+
+public:
+
+	btLowLevelCollisionAlgorithm()
+		:m_manifold(0)
+	{
+	}
+
+	btLowLevelCollisionAlgorithm(btPersistentManifold* manifold, btCollisionDispatcher* dispatcher)
+		:m_manifold(manifold)
+	{
+		m_dispatcher = dispatcher;
+	}
+	virtual ~btLowLevelCollisionAlgorithm()
+	{
+		if (m_manifold)
+			m_dispatcher->releaseManifold(m_manifold);
+	}
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+	{
+		btAssert(0);
+	}
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+	{
+		btAssert(0);
+		return 0.f;
+	}
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		manifoldArray.push_back(m_manifold);
+	}
+	
+};
+
+
+///btLowLevelCollisionDispatcher can use SPU to gather and calculate collision detection
+///Time of Impact, Closest Points and Penetration Depth.
+class btLowLevelCollisionDispatcher : public btCollisionDispatcher
+{
+
+	btLowLevelData* m_lowLevelData;
+
+	btAlignedObjectArray<btPersistentManifold>	m_manifoldArray;
+	btAlignedObjectArray<btLowLevelCollisionAlgorithm> m_algorithms;
+
+protected:
+
+	void	collision();
+
+public:
+
+	
+	btLowLevelCollisionDispatcher (btLowLevelData* lowLevelData, btCollisionConfiguration* collisionConfiguration, int maxNumManifolds);
+	
+	virtual ~btLowLevelCollisionDispatcher();
+
+	bool	supportsDispatchPairOnSpu(int proxyType0,int proxyType1);
+
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
+
+	
+
+	virtual btPersistentManifold*	getNewManifold(void* b0,void* b1)
+	{
+		btAssert(0);
+		return 0;
+	}
+	virtual void releaseManifold(btPersistentManifold* manifold);
+
+	
+	virtual	void* allocateCollisionAlgorithm(int size)
+	{
+		btAssert(0);
+		return 0;
+	}
+
+	virtual	void freeCollisionAlgorithm(void* ptr)
+	{
+
+	}
+
+
+
+};
+
+
+
+#endif //BT_LOW_LEVEL_COLLISION__DISPATCHER_H
+
+

Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelConstraintSolver.h

@@ -0,0 +1,130 @@
+/*
+   Copyright (C) 2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+*/
+
+#ifndef __BT_LOW_LEVEL_CONSTRAINT_SOLVER_H
+#define __BT_LOW_LEVEL_CONSTRAINT_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+
+
+
+
+#include "LinearMath/btScalar.h"
+#include "BulletMultiThreaded/PlatformDefinitions.h"
+#include "physics_effects/low_level/pfx_low_level_include.h"
+#include "../src/low_level/solver/pfx_parallel_group.h"
+
+using namespace sce::PhysicsEffects;
+
+
+class btPersistentManifold;
+
+enum {
+	PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES,
+	PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS,
+	PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS,
+	PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS,
+	PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER
+};
+
+
+struct PfxSetupContactConstraintsIO {
+	PfxConstraintPair *offsetContactPairs;
+	uint32_t numContactPairs1;
+	class TrbState *offsetRigStates;
+	struct PfxSolverBody *offsetSolverBodies;
+	uint32_t numRigidBodies;
+	float separateBias;
+	float timeStep;
+	class btCriticalSection* criticalSection;
+};
+
+
+
+struct PfxSolveConstraintsIO {
+	PfxParallelGroup *contactParallelGroup;
+	PfxParallelBatch *contactParallelBatches;
+	PfxConstraintPair *contactPairs;
+	uint32_t numContactPairs;
+	btPersistentManifold *offsetContactManifolds;
+	PfxParallelGroup *jointParallelGroup;
+	PfxParallelBatch *jointParallelBatches;
+	PfxConstraintPair *jointPairs;
+	uint32_t numJointPairs;
+	TrbState *offsetRigStates;
+	PfxSolverBody *offsetSolverBodies;
+	uint32_t numRigidBodies;
+	uint32_t iteration;
+
+	uint32_t	taskId;
+	
+	class btBarrier* barrier;
+
+};
+
+struct PfxPostSolverIO {
+	TrbState *states;
+	PfxSolverBody *solverBodies;
+	uint32_t numRigidBodies;
+};
+
+ATTRIBUTE_ALIGNED16(struct) btConstraintSolverIO {
+	uint8_t cmd;
+	union {
+		PfxSetupContactConstraintsIO setupContactConstraints;
+		PfxSolveConstraintsIO solveConstraints;
+		PfxPostSolverIO postSolver;
+	};
+	
+	//SPU only
+	uint32_t barrierAddr2;
+	uint32_t criticalsectionAddr2;
+	uint32_t maxTasks1;
+};
+
+
+
+
+void	SolverThreadFunc(void* userPtr,void* lsMemory);
+void*	SolverlsMemoryFunc();
+///The btLowLevelConstraintSolver performs computations on constraint rows in parallel
+///Using the cross-platform threading it supports Windows, Linux, Mac OSX and PlayStation 3 Cell SPUs
+class btLowLevelConstraintSolver : public btSequentialImpulseConstraintSolver
+{
+	
+protected:
+	struct btParallelSolverMemoryCache*	m_memoryCache;
+
+	class btThreadSupportInterface*	m_solverThreadSupport;
+
+	struct btConstraintSolverIO* m_solverIO;
+	class btBarrier*			m_barrier;
+	class btCriticalSection*	m_criticalSection;
+
+
+public:
+
+	btLowLevelConstraintSolver(class btThreadSupportInterface* solverThreadSupport);
+	
+	virtual ~btLowLevelConstraintSolver();
+
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+
+};
+
+
+
+#endif //__BT_LOW_LEVEL_CONSTRAINT_SOLVER_H

Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelConstraintSolver2.cpp

@@ -0,0 +1,190 @@
+/*
+   Copyright (C) 2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+*/
+
+
+#include "btLowLevelConstraintSolver2.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+#include "LinearMath/btQuickprof.h"
+#include "BulletMultiThreaded/btThreadSupportInterface.h"
+#include "BulletMultiThreaded/HeapManager.h"
+#include "BulletMultiThreaded/PlatformDefinitions.h"
+#include "BulletPhysicsEffects/btLowLevelData.h"
+#include "BulletMultiThreaded/vectormath2bullet.h"
+//#include "PfxSimdUtils.h"
+#include "LinearMath/btScalar.h"
+#include "BulletMultiThreaded/HeapManager.h"
+
+
+
+/////////////////
+
+
+#define TMP_BUFF_BYTES (15*1024*1024)
+static unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
+
+
+
+
+btLowLevelConstraintSolver2::btLowLevelConstraintSolver2(btLowLevelData* lowLevelData)
+:m_lowLevelData(lowLevelData)
+{
+	
+}
+	
+btLowLevelConstraintSolver2::~btLowLevelConstraintSolver2()
+{
+	
+}
+
+
+static void solveConstraints(btLowLevelData* lowLevelData, btScalar timeStep, btScalar separateBias, int iteration)
+{
+	PfxPerfCounter pc;
+	HeapManager pool((unsigned char*)tmp_buff,TMP_BUFF_BYTES);
+
+	unsigned int numCurrentPairs = lowLevelData->m_numPairs[lowLevelData->m_pairSwap];
+	PfxBroadphasePair *currentPairs = lowLevelData->m_pairsBuff[lowLevelData->m_pairSwap];
+
+	pc.countBegin("setup solver bodies");
+	{
+		PfxSetupSolverBodiesParam param;
+		param.states = lowLevelData->m_states;
+		param.bodies = lowLevelData->m_bodies;
+		param.solverBodies = lowLevelData->m_solverBodies;
+		param.numRigidBodies = lowLevelData->m_numRigidBodies;
+		
+		int ret = pfxSetupSolverBodies(param);
+		if(ret != SCE_PFX_OK) 
+			SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret);
+	}
+	pc.countEnd();
+
+	pc.countBegin("setup contact constraints");
+	{
+		PfxSetupContactConstraintsParam param;
+		param.contactPairs = currentPairs;
+		param.numContactPairs = numCurrentPairs;
+		param.offsetContactManifolds = lowLevelData->m_contacts;
+		param.offsetRigidStates = lowLevelData->m_states;
+		param.offsetRigidBodies = lowLevelData->m_bodies;
+		param.offsetSolverBodies = lowLevelData->m_solverBodies;
+		param.numRigidBodies = lowLevelData->m_numRigidBodies;
+		param.timeStep = timeStep;
+		param.separateBias = separateBias;
+		
+		int ret = pfxSetupContactConstraints(param);
+		if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
+	}
+	pc.countEnd();
+#if 0
+	pc.countBegin("setup joint constraints");
+	{
+		PfxSetupJointConstraintsParam param;
+		param.jointPairs = 0;//jointPairs;
+		param.numJointPairs = 0;//numJoints;
+		param.offsetJoints = 0;//joints;
+		param.offsetRigidStates = lowLevelData->m_states;
+		param.offsetRigidBodies = lowLevelData->m_bodies;
+		param.offsetSolverBodies = lowLevelData->m_solverBodies;
+		param.numRigidBodies = lowLevelData->m_numRigidBodies;
+		param.timeStep = timeStep;
+
+		for(int i=0;i<numJoints;i++) {
+			pfxUpdateJointPairs(jointPairs[i],i,joints[i],states[joints[i].m_rigidBodyIdA],states[joints[i].m_rigidBodyIdB]);
+		}
+
+		int ret = pfxSetupJointConstraints(param);
+		if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
+	}
+	pc.countEnd();
+#endif
+
+	pc.countBegin("solve constraints");
+	{
+		PfxSolveConstraintsParam param;
+		param.workBytes = pfxGetWorkBytesOfSolveConstraints(lowLevelData->m_numRigidBodies,numCurrentPairs,0);//numJoints);
+		param.workBuff = pool.allocate(param.workBytes);
+		param.contactPairs = currentPairs;
+		param.numContactPairs = numCurrentPairs;
+		param.offsetContactManifolds = lowLevelData->m_contacts;
+		param.jointPairs = 0;//jointPairs;
+		param.numJointPairs = 0;//numJoints;
+		param.offsetJoints = 0;//joints;
+		param.offsetRigidStates = lowLevelData->m_states;
+		param.offsetSolverBodies = lowLevelData->m_solverBodies;
+		param.numRigidBodies = lowLevelData->m_numRigidBodies;
+		param.iteration = iteration;
+
+		int ret = pfxSolveConstraints(param);
+		if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret);
+		
+		pool.deallocate(param.workBuff);
+	}
+	pc.countEnd();
+
+
+	//pc.printCount();
+}
+
+
+
+btScalar btLowLevelConstraintSolver2::solveGroup(btCollisionObject** bodies1,int numRigidBodies,btPersistentManifold** manifoldPtr,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher)
+{
+
+	//copy velocity from BT to PE
+	{
+		BT_PROFILE("copy back");
+		for (int i=0;i<numRigidBodies;i++)
+		{
+			btCollisionObject* obj = bodies1[i];
+			btRigidBody* rb = btRigidBody::upcast(obj);
+			if (rb && (rb->getInvMass()>0.f))
+			{
+				int objectIndex = rb->getBroadphaseProxy()->m_uniqueId;
+				sce::PhysicsEffects::PfxRigidState& state = m_lowLevelData->m_states[objectIndex];
+
+				state.setLinearVelocity(getVmVector3(rb->getLinearVelocity()));
+				state.setAngularVelocity(getVmVector3(rb->getAngularVelocity()));
+
+			}
+		}
+	}
+
+	btScalar separateBias = 0.1f;
+	solveConstraints(m_lowLevelData, infoGlobal.m_timeStep, separateBias, infoGlobal.m_numIterations);
+
+
+	//copy resulting velocity back from PE to BT
+	{
+		BT_PROFILE("copy back");
+		for (int i=0;i<numRigidBodies;i++)
+		{
+			btCollisionObject* obj = bodies1[i];
+			btRigidBody* rb = btRigidBody::upcast(obj);
+			if (rb && (rb->getInvMass()>0.f))
+			{
+				int objectIndex = rb->getBroadphaseProxy()->m_uniqueId;
+				sce::PhysicsEffects::PfxRigidState& state = m_lowLevelData->m_states[objectIndex];
+
+				rb->setLinearVelocity(btVector3(state.getLinearVelocity().getX(),state.getLinearVelocity().getY(),state.getLinearVelocity().getZ()));
+				rb->setAngularVelocity(btVector3(state.getAngularVelocity().getX(),state.getAngularVelocity().getY(),state.getAngularVelocity().getZ()));
+			}
+		}
+	}
+
+	return 0.f;
+}

Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelConstraintSolver2.h

@@ -0,0 +1,55 @@
+/*
+   Copyright (C) 2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+*/
+
+#ifndef __BT_LOW_LEVEL_CONSTRAINT_SOLVER2_H
+#define __BT_LOW_LEVEL_CONSTRAINT_SOLVER2_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+
+
+
+
+#include "LinearMath/btScalar.h"
+#include "BulletMultiThreaded/PlatformDefinitions.h"
+#include "physics_effects/low_level/pfx_low_level_include.h"
+#include "../src/low_level/solver/pfx_parallel_group.h"
+
+using namespace sce::PhysicsEffects;
+
+
+class btPersistentManifold;
+struct btLowLevelData;
+
+class btLowLevelConstraintSolver2 : public btSequentialImpulseConstraintSolver
+{
+	
+protected:
+
+	btLowLevelData*		m_lowLevelData;
+
+public:
+
+	btLowLevelConstraintSolver2(btLowLevelData* lowLevelData);
+	
+	virtual ~btLowLevelConstraintSolver2();
+
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+
+};
+
+
+
+#endif //__BT_LOW_LEVEL_CONSTRAINT_SOLVER2_H

Extras/PhysicsEffects/include/physics_effects.h

@@ -0,0 +1,23 @@
+/*
+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_PHYSICS_EFFECTS_H
+#define _SCE_PHYSICS_EFFECTS_H
+
+#include "physics_effects/low_level/pfx_low_level_include.h"
+#include "physics_effects/util/pfx_util_include.h"
+
+#endif // __PHYSICS_EFFECTS_H

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_common.h

@@ -0,0 +1,181 @@
+/*
+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_COMMON_H
+#define _SCE_PFX_COMMON_H
+
+// Include common headers
+#ifdef _WIN32
+	#include <windows.h>
+	#include <stdio.h>
+	#include <tchar.h>
+#else
+	#include <stdio.h>
+	#include <stdint.h>
+#endif
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+#if defined(_WIN32)
+	#include "pfx_vectormath_include.win32.h"
+#else
+	#include "pfx_vectormath_include.h"
+#endif
+
+namespace sce {
+namespace PhysicsEffects {
+// Basic Types
+#if defined(_WIN32)
+	typedef char               PfxInt8;
+	typedef unsigned char      PfxUInt8;
+	typedef short              PfxInt16;
+	typedef unsigned short     PfxUInt16;
+	typedef int                PfxInt32;
+	typedef unsigned int       PfxUInt32;
+	typedef long long          PfxInt64;
+	typedef unsigned long long PfxUInt64;
+#else
+	typedef int8_t             PfxInt8;
+	typedef uint8_t            PfxUInt8;
+	typedef int16_t            PfxInt16;
+	typedef uint16_t           PfxUInt16;
+	typedef int32_t            PfxInt32;
+	typedef uint32_t           PfxUInt32;
+	typedef int64_t            PfxInt64;
+	typedef uint64_t           PfxUInt64;
+#endif
+
+typedef bool                        PfxBool;
+typedef float                       PfxFloat;
+} //namespace PhysicsEffects
+} //namespace sce
+
+// Debug Print
+#ifdef _WIN32
+static void pfxOutputDebugString(const char *str, ...)
+{
+    char strDebug[1024]={0};
+    va_list argList;
+    va_start(argList, str);
+    vsprintf_s(strDebug,str,argList);
+	OutputDebugStringA(strDebug);
+    va_end(argList);
+}
+#endif
+
+#if defined(_DEBUG)
+	#if defined(_WIN32)
+		#define SCE_PFX_DPRINT pfxOutputDebugString
+	#else
+		#define SCE_PFX_DPRINT(...) printf(__VA_ARGS__)
+	#endif
+#else
+	#ifdef _WIN32
+		#define SCE_PFX_DPRINT
+	#else
+		#define SCE_PFX_DPRINT(...)
+	#endif
+#endif
+
+// Printf
+#if defined(_WIN32)
+	#define SCE_PFX_PRINTF pfxOutputDebugString
+#else
+// ARA begin insert new code
+	#ifdef __ANDROID__
+		#include <android/log.h>
+		#define SCE_PFX_PRINTF(...) __android_log_print(ANDROID_LOG_VERBOSE, "SCE_PFX_PRINTF", __VA_ARGS__)
+	#else
+		#define SCE_PFX_PRINTF(...) printf(__VA_ARGS__)
+	#endif
+// ARA end, old baseline code block was:
+//	#define SCE_PFX_PRINTF(...) printf(__VA_ARGS__)
+//
+#endif
+
+#define SCE_PFX_UNLIKELY(a)		(a)
+#define SCE_PFX_LIKELY(a)		(a)
+
+// Inline
+#if defined(_MSC_VER)
+	#define SCE_PFX_FORCE_INLINE __forceinline
+#elif defined(__SNC__) || defined(__GNUC__)
+	#define SCE_PFX_FORCE_INLINE inline __attribute__((always_inline))
+#endif
+
+// Assert
+#define SCE_PFX_HALT() abort()
+
+#ifdef _DEBUG
+	#define SCE_PFX_ASSERT(test) {if(!(test)){SCE_PFX_PRINTF("Assert "__FILE__ ":%u ("#test")\n", __LINE__);SCE_PFX_HALT();}}
+	#define SCE_PFX_ASSERT_MSG(test,msg) {if(!(test)){SCE_PFX_PRINTF("Assert " msg " " __FILE__ ":%u ("#test")\n",__LINE__);SCE_PFX_HALT();}}
+#else
+	#define SCE_PFX_ASSERT(test)
+	#define SCE_PFX_ASSERT_MSG(test,msg)
+#endif
+
+#define SCE_PFX_ALWAYS_ASSERT(test) {if(!(test)){SCE_PFX_PRINTF("Assert "__FILE__ ":%u ("#test")\n", __LINE__);SCE_PFX_HALT();}}
+#define SCE_PFX_ALWAYS_ASSERT_MSG(test,msg) {if(!(test)){SCE_PFX_PRINTF("Assert:" msg " " __FILE__ ":%u ("#test")\n",__LINE__);SCE_PFX_HALT();}}
+
+// Aligned 
+#if defined(_MSC_VER)
+	#define SCE_PFX_ALIGNED(alignment)   __declspec(align(alignment))
+#elif defined(__SNC__) || defined(__GNUC__)
+	#define SCE_PFX_ALIGNED(alignment)   __attribute__((__aligned__((alignment))))
+#endif
+
+// Etc
+#define SCE_PFX_MIN(a,b) (((a)<(b))?(a):(b))
+#define SCE_PFX_MAX(a,b) (((a)>(b))?(a):(b))
+#define SCE_PFX_CLAMP(v,a,b) SCE_PFX_MAX(a,SCE_PFX_MIN(v,b))
+#define SCE_PFX_SWAP(type, x, y) do {type t; t=x; x=y; y=t; } while (0)
+#define SCE_PFX_SQR(a) ((a)*(a))
+
+#define SCE_PFX_ALIGN16(count,size)   ((((((count) * (size)) + 15) & (~15)) + (size)-1) / (size))
+#define SCE_PFX_ALIGN128(count,size)  ((((((count) * (size)) + 127) & (~127)) + (size)-1) / (size))
+
+#define SCE_PFX_AVAILABLE_BYTES_ALIGN16(ptr,bytes) (bytes-((uintptr_t)(ptr)&0x0f))
+#define SCE_PFX_AVAILABLE_BYTES_ALIGN128(ptr,bytes) (bytes-((uintptr_t)(ptr)&0x7f))
+
+#define SCE_PFX_BYTES_ALIGN16(bytes) (((bytes)+15)&(~15))
+#define SCE_PFX_BYTES_ALIGN128(bytes) (((bytes)+127)&(~127))
+
+#define SCE_PFX_PTR_ALIGN16(ptr) (((uintptr_t)(ptr)+15)&(~15))
+#define SCE_PFX_PTR_ALIGN128(ptr) (((uintptr_t)(ptr)+127)&(~127))
+
+#define SCE_PFX_PTR_IS_ALIGNED16(ptr) (((uintptr_t)(ptr)&0x0f)==0)
+#define SCE_PFX_PTR_IS_ALIGNED128(ptr) (((uintptr_t)(ptr)&0x7f)==0)
+
+#define SCE_PFX_GET_POINTER(offset,stride,id) ((uintptr_t)(offset)+(stride)*(id))
+
+#define SCE_PFX_FLT_MAX 1e+38f
+#define SCE_PFX_PI 3.14159265358979f
+
+#define SCE_PFX_RANGE_CHECK(val,minVal,maxVal) (((val)>=(minVal))&&((val)<=(maxVal)))
+
+#define SCE_PFX_IS_RUNNING_ON_64BIT_ENV() ( ( sizeof(void*)==8 )? true : false )
+
+#if defined(__SNC__) || defined(__GNUC__)
+	#define SCE_PFX_PADDING(count,bytes) PfxUInt8 padding##count[bytes];
+#else
+	#define SCE_PFX_PADDING(count,bytes)
+#endif
+
+#include "pfx_error_code.h"
+
+#endif // _SCE_PFX_COMMON_H

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_error_code.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_ERROR_CODE_H
+#define _SCE_PFX_ERROR_CODE_H
+
+#define SCE_PFX_OK 								0
+#define SCE_PFX_ERR_INVALID_VALUE				0x80880001
+#define SCE_PFX_ERR_INVALID_ALIGN				0x80880002
+#define SCE_PFX_ERR_OUT_OF_BUFFER				0x80880003
+#define SCE_PFX_ERR_OUT_OF_MAX_PAIRS			0x80880004
+#define SCE_PFX_ERR_OUT_OF_RANGE				0x80880005
+#define SCE_PFX_ERR_OUT_OF_WORLD				0x80880006
+#define SCE_PFX_ERR_INVALID_FLAG				0x80880007
+
+#endif // _SCE_PFX_ERROR_CODE_H
+

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_heap_manager.h

@@ -0,0 +1,141 @@
+/*
+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_HEAP_MANAGER_H
+#define _SCE_PFX_HEAP_MANAGER_H
+
+#include "pfx_common.h"
+
+//J プールされたメモリを管理するスタックのサイズ
+//E Size of a stack which used to manage pool memory
+#define SCE_PFX_HEAP_STACK_SIZE 64
+
+#define SCE_PFX_MIN_ALLOC_SIZE 16
+
+#define SCE_PFX_ALLOC_BYTES_ALIGN16(bytes) SCE_PFX_MAX(16,SCE_PFX_BYTES_ALIGN16(bytes))
+#define SCE_PFX_ALLOC_BYTES_ALIGN128(bytes) SCE_PFX_MAX(128,SCE_PFX_BYTES_ALIGN128(bytes))
+
+#if defined (_WIN64) || defined (__LP64__)
+	#define SCE_PFX_ALIGN_MASK_16	0xfffffffffffffff0
+	#define SCE_PFX_ALIGN_MASK_128	0xffffffffffffff80
+#else
+	#define SCE_PFX_ALIGN_MASK_16	0xfffffff0
+	#define SCE_PFX_ALIGN_MASK_128	0xffffff80
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// PfxHeapManager
+
+//J ＜補足＞
+//J メモリはスタックで管理されています。取得した順と逆に開放する必要があります。
+//J メモリを一気に開放したい場合はclear()を呼び出してください。
+//J 最小割り当てサイズはSCE_PFX_MIN_ALLOC_SIZEで定義されます。
+
+//E <Notes>
+//E Memory is managed as a stack, so deallocate() needs to be called in reverse order.
+//E Use clear() to deallocate all allocated memory at once.
+//E SCE_PFX_MIN_ALLOC_SIZE defines the smallest amount of buffer.
+
+namespace sce {
+namespace PhysicsEffects {
+
+class PfxHeapManager
+{
+private:
+	PfxUInt8 *m_heap;
+	PfxUInt8 *m_poolStack[SCE_PFX_HEAP_STACK_SIZE];
+	PfxInt32 m_heapBytes;
+	PfxInt32 m_curStack;
+	PfxInt32 m_rest;
+	
+public:
+	enum {ALIGN16=16,ALIGN128=128};
+
+	PfxHeapManager(PfxUInt8 *buf,PfxInt32 bytes)
+	{
+		m_heap = buf;
+		m_heapBytes = bytes;
+		clear();
+	}
+	
+	~PfxHeapManager()
+	{
+	}
+	
+	PfxInt32 getAllocated()
+	{
+		return (PfxInt32)(m_poolStack[m_curStack]-m_heap);
+	}
+	
+	PfxInt32 getRest()
+	{
+		return m_heapBytes-getAllocated();
+	}
+
+	void *allocate(size_t bytes,PfxInt32 alignment = ALIGN16)
+	{
+	SCE_PFX_ALWAYS_ASSERT(m_curStack<SCE_PFX_HEAP_STACK_SIZE);
+
+	bytes = SCE_PFX_MAX(bytes,SCE_PFX_MIN_ALLOC_SIZE);
+
+	uintptr_t p = (uintptr_t)m_poolStack[m_curStack];
+
+	if(alignment == ALIGN128) {
+		p = (p+127) & SCE_PFX_ALIGN_MASK_128;
+		bytes = (bytes+127) & SCE_PFX_ALIGN_MASK_128;
+	}
+	else {
+		p = (p+15) & SCE_PFX_ALIGN_MASK_16;
+		bytes = (bytes+15) & SCE_PFX_ALIGN_MASK_16;
+	}
+
+	SCE_PFX_ALWAYS_ASSERT_MSG(bytes <= (m_heapBytes-(p-(uintptr_t)m_heap)),"Memory overflow");
+
+	m_poolStack[++m_curStack] = (PfxUInt8 *)(p + bytes);
+
+	m_rest = getRest();
+
+	return (void*)p;
+	}
+
+	void deallocate(void *p)
+	{
+#if 0
+		m_curStack--;
+		PfxInt32 addr = (PfxInt32)m_poolStack[m_curStack];
+		SCE_PFX_ALWAYS_ASSERT_MSG(addr == (PfxInt32)p || ((addr+127) & 0xffffff80) == (PfxInt32)p,"Stack overflow");
+#else
+		(void) p;
+		m_curStack--;
+#endif
+	}
+	
+	void clear()
+	{
+		m_poolStack[0] = m_heap;
+		m_curStack = 0;
+		m_rest = 0;
+	}
+
+	void printStack()
+	{
+		SCE_PFX_PRINTF("memStack %d/%d\n",m_curStack,SCE_PFX_HEAP_STACK_SIZE);
+	}
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_HEAP_MANAGER_H

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_perf_counter.h

@@ -0,0 +1,168 @@
+/*
+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_PERF_COUNTER_H
+#define _SCE_PFX_PERF_COUNTER_H
+
+#include "pfx_common.h"
+
+// ARA begin insert new code
+#ifndef _WIN32
+#include <time.h>
+#endif
+// ARA end
+
+//J パフォーマンス測定する場合はPFX_USE_PERFCOUNTERを定義
+//J ブックマークを使用する場合はPFX_USE_BOOKMARKを定義
+
+//E Define SCE_PFX_USE_PERFCOUNTER to check performance
+//E Define SCE_PFX_USE_BOOKMARK to use bookmark
+
+
+#define SCE_PFX_MAX_PERF_STR	32
+#define SCE_PFX_MAX_PERF_COUNT	20
+
+//#define SCE_PFX_USE_PERFCOUNTER
+//#define SCE_PFX_USE_BOOKMARK
+
+namespace sce {
+namespace PhysicsEffects {
+#ifdef SCE_PFX_USE_PERFCOUNTER
+
+class PfxPerfCounter
+{
+private:
+	int   m_count,m_strCount;
+	char  m_str[SCE_PFX_MAX_PERF_COUNT][SCE_PFX_MAX_PERF_STR];
+	float m_freq;
+
+	SCE_PFX_PADDING(1,4)
+#ifdef _WIN32
+	LONGLONG  m_cnt[SCE_PFX_MAX_PERF_COUNT*2];
+#else
+// ARA begin insert new code
+	timespec m_cnt[SCE_PFX_MAX_PERF_COUNT*2];
+// ARA end
+#endif
+
+	void count(int i)
+	{
+#ifdef _WIN32
+		QueryPerformanceCounter( (LARGE_INTEGER *)&m_cnt[i] );
+#else
+// ARA begin insert new code
+		clock_gettime(CLOCK_MONOTONIC, &m_cnt[i]);
+// ARA end
+#endif
+	}
+
+public:
+	PfxPerfCounter()
+	{
+#ifdef _WIN32
+		LARGE_INTEGER sPerfCountFreq;
+		QueryPerformanceFrequency(&sPerfCountFreq);
+		m_freq = (float)sPerfCountFreq.QuadPart;
+#else
+// ARA begin insert new code 
+		m_freq = 1000000000.0f; // clock_gettime reports time in nanoseconds (though accuracy is platform dependent)
+// ARA end
+#endif
+		resetCount();
+	}
+
+	~PfxPerfCounter()
+	{
+		//printCount();
+	}
+
+	void countBegin(const char *name)
+	{
+		SCE_PFX_ASSERT(m_strCount < SCE_PFX_MAX_PERF_COUNT);
+		strncpy(m_str[m_strCount],name,SCE_PFX_MAX_PERF_STR-1);
+		m_str[m_strCount][SCE_PFX_MAX_PERF_STR-1] = 0x00;
+		m_strCount++;
+		count(m_count++);
+	}
+	
+	void countEnd()
+	{
+		count(m_count++);
+	}
+
+	void resetCount()
+	{
+		m_strCount = 0;
+		m_count = 0;
+	}
+
+	float getCountTime(int i)
+	{
+#if _WIN32
+	return (float)(m_cnt[i+1]-m_cnt[i]) / m_freq * 1000.0f;
+#else
+// ARA begin insert new code 
+	return float(m_cnt[i+1].tv_sec - m_cnt[i].tv_sec) +
+				 (float(m_cnt[i+1].tv_nsec - m_cnt[i].tv_nsec) / m_freq);
+// ARA end
+#endif	
+}
+
+	void printCount()
+	{
+		if(m_count%2 != 0) countEnd();
+		SCE_PFX_PRINTF("*** PfxPerfCounter results ***\n");
+		float total = 0.0f;
+		for(int i=0;i+1<m_count;i+=2) {
+			total += getCountTime(i);
+		}
+		for(int i=0;i+1<m_count;i+=2) {
+			SCE_PFX_PRINTF(" -- %s %fms(%.2f%%)\n",m_str[i>>1],getCountTime(i),getCountTime(i)/total*100.0f);
+		}
+		SCE_PFX_PRINTF(" -- Total %fms\n",total);
+	}
+};
+
+#else /* SCE_PFX_USE_PERFCOUNTER */
+
+class PfxPerfCounter
+{
+public:
+	PfxPerfCounter() {}
+	~PfxPerfCounter() {}
+	void countBegin(const char *name) {(void) name;}
+	void countEnd() {}
+	void resetCount() {}
+	float getCountTime(int i) {(void)i;return 0.0f;}
+	void printCount() {}
+};
+
+#endif /* SCE_PFX_USE_PERFCOUNTER */
+
+#define pfxInsertBookmark(bookmark)
+
+#ifdef SCE_PFX_USE_BOOKMARK
+	#define SCE_PFX_PUSH_MARKER(name)
+	#define SCE_PFX_POP_MARKER()
+#else
+	#define SCE_PFX_PUSH_MARKER(name)
+	#define SCE_PFX_POP_MARKER()
+#endif
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_PERF_COUNTER_H

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_vec_int3.h

@@ -0,0 +1,166 @@
+/*
+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_VEC_INT3_H
+#define _SCE_PFX_VEC_INT3_H
+
+#include "pfx_common.h"
+
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+class SCE_PFX_ALIGNED(16) PfxVecInt3
+{
+private:
+PfxInt32 m_x,m_y,m_z,m_w;
+
+public:
+PfxVecInt3() {m_x=m_y=m_z=m_w=0;}
+PfxVecInt3(const PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG vec) {m_x=(PfxInt32)vec[0];m_y=(PfxInt32)vec[1];m_z=(PfxInt32)vec[2];m_w=0;}
+PfxVecInt3(PfxFloat fx,PfxFloat fy,PfxFloat fz) {m_x=(PfxInt32)fx;m_y=(PfxInt32)fy;m_z=(PfxInt32)fz;m_w=0;}
+PfxVecInt3(PfxInt32 iv) {m_x=m_y=m_z=iv;m_w=0;}
+PfxVecInt3(PfxInt32 ix,PfxInt32 iy,PfxInt32 iz) {m_x=ix;m_y=iy;m_z=iz;m_w=0;}
+
+    inline PfxVecInt3 &operator =( const PfxVecInt3 &vec);
+
+inline PfxInt32 get(PfxInt32 i) const {return *(&m_x+i);}
+inline PfxInt32 getX() const {return m_x;}
+inline PfxInt32 getY() const {return m_y;}
+inline PfxInt32 getZ() const {return m_z;}
+inline void set(PfxInt32 i,PfxInt32 v) {*(&m_x+i) = v;}
+inline void setX(PfxInt32 v) {m_x = v;}
+inline void setY(PfxInt32 v) {m_y = v;}
+inline void setZ(PfxInt32 v) {m_z = v;}
+
+    inline const PfxVecInt3 operator +( const PfxVecInt3 & vec ) const;
+    inline const PfxVecInt3 operator -( const PfxVecInt3 & vec ) const;
+    inline const PfxVecInt3 operator *( PfxInt32 scalar ) const;
+    inline const PfxVecInt3 operator /( PfxInt32 scalar ) const;
+
+    inline PfxVecInt3 & operator +=( const PfxVecInt3 & vec );
+    inline PfxVecInt3 & operator -=( const PfxVecInt3 & vec );
+    inline PfxVecInt3 & operator *=( PfxInt32 scalar );
+    inline PfxVecInt3 & operator /=( PfxInt32 scalar );
+
+    inline const PfxVecInt3 operator -() const;
+
+operator PfxVector3() const
+{
+	return PfxVector3((PfxFloat)m_x,(PfxFloat)m_y,(PfxFloat)m_z);
+}
+};
+
+inline PfxVecInt3 &PfxVecInt3::operator =( const PfxVecInt3 &vec)
+{
+    m_x = vec.m_x;
+    m_y = vec.m_y;
+    m_z = vec.m_z;
+    return *this;
+}
+
+inline const PfxVecInt3 PfxVecInt3::operator +( const PfxVecInt3 & vec ) const
+{
+    return PfxVecInt3(m_x+vec.m_x, m_y+vec.m_y, m_z+vec.m_z);
+}
+
+inline const PfxVecInt3 PfxVecInt3::operator -( const PfxVecInt3 & vec ) const
+{
+    return PfxVecInt3(m_x-vec.m_x, m_y-vec.m_y, m_z-vec.m_z);
+}
+
+inline const PfxVecInt3 PfxVecInt3::operator *( PfxInt32 scalar ) const
+{
+    return PfxVecInt3(m_x*scalar, m_y*scalar, m_z*scalar);
+}
+
+inline const PfxVecInt3 PfxVecInt3::operator /( PfxInt32 scalar ) const
+{
+    return PfxVecInt3(m_x/scalar, m_y/scalar, m_z/scalar);
+}
+
+inline PfxVecInt3 &PfxVecInt3::operator +=( const PfxVecInt3 & vec )
+{
+    *this = *this + vec;
+    return *this;
+}
+
+inline PfxVecInt3 &PfxVecInt3::operator -=( const PfxVecInt3 & vec )
+{
+    *this = *this - vec;
+    return *this;
+}
+
+inline PfxVecInt3 &PfxVecInt3::operator *=( PfxInt32 scalar )
+{
+    *this = *this * scalar;
+    return *this;
+}
+
+inline PfxVecInt3 &PfxVecInt3::operator /=( PfxInt32 scalar )
+{
+    *this = *this / scalar;
+    return *this;
+}
+
+inline const PfxVecInt3 PfxVecInt3::operator -() const
+{
+return PfxVecInt3(-m_x,-m_y,-m_z);
+}
+
+inline const PfxVecInt3 operator *( PfxInt32 scalar, const PfxVecInt3 & vec )
+{
+    return vec * scalar;
+}
+
+inline const PfxVecInt3 mulPerElem( const PfxVecInt3 & vec0, const PfxVecInt3 & vec1 )
+{
+return PfxVecInt3(vec0.getX()*vec1.getX(), vec0.getY()*vec1.getY(), vec0.getZ()*vec1.getZ());
+}
+
+inline const PfxVecInt3 divPerElem( const PfxVecInt3 & vec0, const PfxVecInt3 & vec1 )
+{
+return PfxVecInt3(vec0.getX()/vec1.getX(), vec0.getY()/vec1.getY(), vec0.getZ()/vec1.getZ());
+}
+
+inline const PfxVecInt3 absPerElem( const PfxVecInt3 & vec )
+{
+return PfxVecInt3(abs(vec.getX()), abs(vec.getY()), abs(vec.getZ()));
+}
+
+inline const PfxVecInt3 maxPerElem( const PfxVecInt3 & vec0, const PfxVecInt3 & vec1 )
+{
+    return PfxVecInt3(
+        (vec0.getX() > vec1.getX())? vec0.getX() : vec1.getX(),
+        (vec0.getY() > vec1.getY())? vec0.getY() : vec1.getY(),
+        (vec0.getZ() > vec1.getZ())? vec0.getZ() : vec1.getZ()
+    );
+}
+
+inline const PfxVecInt3 minPerElem( const PfxVecInt3 & vec0, const PfxVecInt3 & vec1 )
+{
+    return PfxVecInt3(
+        (vec0.getX() < vec1.getX())? vec0.getX() : vec1.getX(),
+        (vec0.getY() < vec1.getY())? vec0.getY() : vec1.getY(),
+        (vec0.getZ() < vec1.getZ())? vec0.getZ() : vec1.getZ()
+    );
+}
+} //namespace PhysicsEffects
+} //namespace sce
+
+
+#endif // _SCE_PFX_VEC_INT3_H

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_vec_utils.h

@@ -0,0 +1,181 @@
+/*
+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_VEC_UTILS_H
+#define _SCE_PFX_VEC_UTILS_H
+
+#include "pfx_common.h"
+#include "pfx_vec_int3.h"
+
+namespace sce {
+namespace PhysicsEffects {
+static SCE_PFX_FORCE_INLINE PfxVector3 pfxReadVector3(const PfxFloat* fptr)
+{
+	PfxVector3 v;
+	
+loadXYZ(v, fptr);
+
+	return v;
+}
+
+static SCE_PFX_FORCE_INLINE PfxPoint3 pfxReadPoint3(const PfxFloat* fptr)
+{
+	PfxPoint3 v;
+
+loadXYZ(v, fptr);
+
+	return v;
+}
+
+static SCE_PFX_FORCE_INLINE PfxQuat pfxReadQuat(const PfxFloat* fptr)
+{
+	PfxQuat vq;
+
+loadXYZW(vq, fptr);
+
+	return vq;
+}
+
+static SCE_PFX_FORCE_INLINE void pfxStoreVector3(const PfxVector3 &src, PfxFloat* fptr)
+{
+storeXYZ(src, fptr);
+}
+
+static SCE_PFX_FORCE_INLINE void pfxStorePoint3(const PfxPoint3 &src, PfxFloat* fptr)
+{
+storeXYZ(src, fptr);
+}
+
+static SCE_PFX_FORCE_INLINE void pfxStoreQuat(const PfxQuat &src, PfxFloat* fptr)
+{
+storeXYZW(src, fptr);
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+namespace sce {
+namespace PhysicsEffects {
+static SCE_PFX_FORCE_INLINE
+void pfxGetPlaneSpace(const PfxVector3& n, PfxVector3& fptr, PfxVector3& q)
+{
+	if(fabsf(n[2]) > 0.707f) {
+		// choose fptr in y-z plane
+		PfxFloat a = n[1]*n[1] + n[2]*n[2];
+		PfxFloat k = 1.0f/sqrtf(a);
+		fptr[0] = 0;
+		fptr[1] = -n[2]*k;
+		fptr[2] = n[1]*k;
+		// set q = n x fptr
+		q[0] = a*k;
+		q[1] = -n[0]*fptr[2];
+		q[2] = n[0]*fptr[1];
+	}
+	else {
+		// choose fptr in x-y plane
+		PfxFloat a = n[0]*n[0] + n[1]*n[1];
+		PfxFloat k = 1.0f/sqrtf(a);
+		fptr[0] = -n[1]*k;
+		fptr[1] = n[0]*k;
+		fptr[2] = 0;
+		// set q = n x fptr
+		q[0] = -n[2]*fptr[1];
+		q[1] = n[2]*fptr[0];
+		q[2] = a*k;
+	}
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxGetRotationAngleAndAxis(const PfxQuat &unitQuat,PfxFloat &angle,PfxVector3 &axis)
+{
+	const PfxFloat epsilon=0.00001f;
+
+	if(fabsf(unitQuat.getW()) < 1.0f-epsilon && lengthSqr(unitQuat.getXYZ()) > epsilon) {
+		PfxFloat angleHalf = acosf(unitQuat.getW());
+		PfxFloat sinAngleHalf = sinf(angleHalf);
+
+		if(fabsf(sinAngleHalf) > 1.0e-10f) {
+			axis = unitQuat.getXYZ()/sinAngleHalf;
+		} else {
+			axis = unitQuat.getXYZ();
+		}
+		angle = 2.0f*angleHalf;
+	} else {
+		angle = 0.0f;
+		axis = PfxVector3(1.0f, 0.0f, 0.0f);
+	}
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxFloat pfxSafeAtan2(PfxFloat y,PfxFloat x)
+{
+	if(SCE_PFX_SQR(x) < 0.000001f || SCE_PFX_SQR(y) < 0.000001f) {
+		return 0.0f;
+	}
+	return atan2f(y,x);
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxVector3 pfxSafeNormalize(const PfxVector3 &vec)
+{
+	float lenSqr = lengthSqr( vec );
+
+	if( lenSqr > 0.000001f ) {
+		return normalize(vec);
+	}else {
+		return PfxVector3( 1.0f, 0.0f, 0.0f );
+	}
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxVecInt3 pfxConvertCoordWorldToLocal(const PfxVector3 &coord,const PfxVector3 &center,const PfxVector3 &half)
+{
+	const PfxVector3 sz(65535.0f);
+	PfxVector3 q = divPerElem(coord - center + half,2.0f*half);
+	q = minPerElem(maxPerElem(q,PfxVector3(0.0f)),PfxVector3(1.0f)); // clamp 0.0 - 1.0
+	q = mulPerElem(q,sz);
+	return PfxVecInt3(q);
+}
+
+static SCE_PFX_FORCE_INLINE
+void pfxConvertCoordWorldToLocal(
+	const PfxVector3 &center,const PfxVector3 &half,
+	const PfxVector3 &coordMin,const PfxVector3 &coordMax,
+	PfxVecInt3 &localMin,PfxVecInt3 &localMax)
+{
+	const PfxVector3 sz(65535.0f);
+	PfxVector3 qmin = divPerElem(coordMin - center + half,2.0f*half);
+	qmin = minPerElem(maxPerElem(qmin,PfxVector3(0.0f)),PfxVector3(1.0f)); // clamp 0.0 - 1.0
+	qmin = mulPerElem(qmin,sz);
+	
+	PfxVector3 qmax = divPerElem(coordMax - center + half,2.0f*half);
+	qmax = minPerElem(maxPerElem(qmax,PfxVector3(0.0f)),PfxVector3(1.0f)); // clamp 0.0 - 1.0
+	qmax = mulPerElem(qmax,sz);
+localMin = PfxVecInt3(floorf(qmin[0]),floorf(qmin[1]),floorf(qmin[2]));
+localMax = PfxVecInt3(ceilf(qmax[0]),ceilf(qmax[1]),ceilf(qmax[2]));
+}
+
+static SCE_PFX_FORCE_INLINE
+PfxVector3 pfxConvertCoordLocalToWorld(const PfxVecInt3 &coord,const PfxVector3 &center,const PfxVector3 &half)
+{
+	PfxVector3 sz(65535.0f),vcoord(coord);
+	PfxVector3 q = divPerElem(vcoord,sz);
+	return mulPerElem(q,2.0f*half) + center - half;
+}
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_VEC_UTILS_H

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_vectormath_include.h

@@ -0,0 +1,101 @@
+/*
+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_VECTORMATH_INCLUDE_H
+#define _SCE_PFX_VECTORMATH_INCLUDE_H
+
+// If you want to use the free/open sourced vectormath, you need to
+// put codes in the include/vecmath folder and set following define.
+#define SCE_PFX_USE_FREE_VECTORMATH
+
+// If you want to use vectomath with SIMD,
+// following define is needed.
+//#define SCE_PFX_USE_SIMD_VECTORMATH
+
+// This option enables to replace original implementation with 
+// vector geometry library.
+//#define SCE_PFX_USE_GEOMETRY
+
+// vectormath include
+#ifdef SCE_PFX_USE_FREE_VECTORMATH
+	// ARA begin insert new code
+	#if defined(SCE_PFX_USE_SIMD_VECTORMATH) && defined(__ANDROID__) && defined(__ARM_NEON__)
+		// For Android targets supporting NEON instructions,
+		// use NEON-optimized vector math library
+		#include "../../../vecmath/neon/vectormath_aos.h"
+		#include "../../../vecmath/neon/floatInVec.h"
+	#else
+	// ARA end
+		// use standard free vector math library
+		#include "../../../vecmath/std/vectormath_aos.h"
+		#include "../../../vecmath/std/floatInVec.h"
+	// ARA begin insert new code 
+	#endif
+	// ARA end
+	#define SCE_VECTORMATH_AOS_VECTOR_ARG 
+	#define SCE_VECTORMATH_AOS_MATRIX_ARG 
+
+	namespace sce {
+	namespace PhysicsEffects {
+	typedef Vectormath::Aos::Point3     PfxPoint3;
+	typedef Vectormath::Aos::Vector3    PfxVector3;
+	typedef Vectormath::Aos::Vector4    PfxVector4;
+	typedef Vectormath::Aos::Quat       PfxQuat;
+	typedef Vectormath::Aos::Matrix3    PfxMatrix3;
+	typedef Vectormath::Aos::Matrix4    PfxMatrix4;
+	typedef Vectormath::Aos::Transform3 PfxTransform3;
+	typedef Vectormath::floatInVec		PfxFloatInVec;
+	typedef Vectormath::boolInVec		PfxBoolInVec;
+	} //namespace PhysicsEffects
+	} //namespace sce
+#else
+	#include <vectormath.h>
+	#define SCE_GEOMETRY_USE_SCALAR_MATH
+	#define SCE_VECTORMATH_AOS_VECTOR_ARG SCE_VECTORMATH_SCALAR_AOS_VECTOR_ARG
+	#define SCE_VECTORMATH_AOS_MATRIX_ARG SCE_VECTORMATH_SCALAR_AOS_MATRIX_ARG
+
+	namespace sce {
+	namespace PhysicsEffects {
+	typedef sce::Vectormath::Scalar::Aos::Point3     PfxPoint3;
+	typedef sce::Vectormath::Scalar::Aos::Vector3    PfxVector3;
+	typedef sce::Vectormath::Scalar::Aos::Vector4    PfxVector4;
+	typedef sce::Vectormath::Scalar::Aos::Quat       PfxQuat;
+	typedef sce::Vectormath::Scalar::Aos::Matrix3    PfxMatrix3;
+	typedef sce::Vectormath::Scalar::Aos::Matrix4    PfxMatrix4;
+	typedef sce::Vectormath::Scalar::Aos::Transform3 PfxTransform3;
+	typedef sce::Vectormath::Scalar::floatInVec		PfxFloatInVec;
+	typedef sce::Vectormath::Scalar::boolInVec		PfxBoolInVec;
+	} //namespace PhysicsEffects
+	} //namespace sce
+
+	#ifdef SCE_PFX_USE_GEOMETRY
+	#include <sce_geometry.h>
+	namespace sce {
+	namespace PhysicsEffects {
+	typedef sce::Geometry::Aos::Line	PfxGeomLine;
+	typedef sce::Geometry::Aos::Segment	PfxGeomSegment;
+	typedef sce::Geometry::Aos::Plane	PfxGeomPlane;
+	typedef sce::Geometry::Aos::Sphere	PfxGeomSphere;
+	typedef sce::Geometry::Aos::Capsule	PfxGeomCapsule;
+	typedef sce::Geometry::Aos::Bounds	PfxGeomBounds;
+	typedef sce::Geometry::Aos::Aabb	PfxGeomAabb;
+	typedef sce::Geometry::Aos::Obb		PfxGeomObb;
+	} //namespace PhysicsEffects
+	} //namespace sce
+	#endif
+#endif
+
+#endif // _SCE_PFX_VECTORMATH_INCLUDE_H

Extras/PhysicsEffects/include/physics_effects/base_level/base/pfx_vectormath_include.win32.h

@@ -0,0 +1,116 @@
+/*
+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_VECTORMATH_INCLUDE_WIN32_H
+#define _SCE_PFX_VECTORMATH_INCLUDE_WIN32_H
+
+// If you want to use the free/open sourced vectormath, you need to
+// put codes in the include/vecmath folder and set following define.
+#define SCE_PFX_USE_FREE_VECTORMATH
+
+// If you want to use vectomath with SIMD,
+// following define is needed.
+// #define SCE_PFX_USE_SIMD_VECTORMATH
+
+// This option enables to replace original implementation with 
+// vector geometry library.
+//#define SCE_PFX_USE_GEOMETRY
+
+// vectormath include
+#ifdef SCE_PFX_USE_FREE_VECTORMATH
+	#ifdef SCE_PFX_USE_SIMD_VECTORMATH
+		#include "../../../vecmath/sse/vectormath_aos.h"
+		#include "../../../vecmath/sse/floatInVec.h"
+		#define SCE_VECTORMATH_AOS_VECTOR_ARG &
+		#define SCE_VECTORMATH_AOS_MATRIX_ARG &
+	#else
+		#include "../../../vecmath/std/vectormath_aos.h"
+		#include "../../../vecmath/std/floatInVec.h"
+		#define SCE_VECTORMATH_AOS_VECTOR_ARG
+		#define SCE_VECTORMATH_AOS_MATRIX_ARG
+	#endif
+
+	namespace sce {
+	namespace PhysicsEffects {
+	typedef Vectormath::Aos::Point3     PfxPoint3;
+	typedef Vectormath::Aos::Vector3    PfxVector3;
+	typedef Vectormath::Aos::Vector4    PfxVector4;
+	typedef Vectormath::Aos::Quat       PfxQuat;
+	typedef Vectormath::Aos::Matrix3    PfxMatrix3;
+	typedef Vectormath::Aos::Matrix4    PfxMatrix4;
+	typedef Vectormath::Aos::Transform3 PfxTransform3;
+	typedef Vectormath::floatInVec		 PfxFloatInVec;
+	typedef Vectormath::boolInVec		 PfxBoolInVec;
+	} //namespace PhysicsEffects
+	} //namespace sce
+#else
+	#include <vectormath.h>
+
+	#ifdef SCE_PFX_USE_SIMD_VECTORMATH
+		#define SCE_VECTORMATH_AOS_VECTOR_ARG SCE_VECTORMATH_SIMD_AOS_VECTOR_ARG
+		#define SCE_VECTORMATH_AOS_MATRIX_ARG SCE_VECTORMATH_SIMD_AOS_MATRIX_ARG
+
+		namespace sce {
+		namespace PhysicsEffects {
+		typedef sce::Vectormath::Simd::Aos::Point3     PfxPoint3;
+		typedef sce::Vectormath::Simd::Aos::Vector3    PfxVector3;
+		typedef sce::Vectormath::Simd::Aos::Vector4    PfxVector4;
+		typedef sce::Vectormath::Simd::Aos::Quat       PfxQuat;
+		typedef sce::Vectormath::Simd::Aos::Matrix3    PfxMatrix3;
+		typedef sce::Vectormath::Simd::Aos::Matrix4    PfxMatrix4;
+		typedef sce::Vectormath::Simd::Aos::Transform3 PfxTransform3;
+		typedef sce::Vectormath::Simd::floatInVec		PfxFloatInVec;
+		typedef sce::Vectormath::Simd::boolInVec		PfxBoolInVec;
+		} //namespace PhysicsEffects
+		} //namespace sce
+	#else
+		#define SCE_GEOMETRY_USE_SCALAR_MATH
+		#define SCE_VECTORMATH_AOS_VECTOR_ARG SCE_VECTORMATH_SCALAR_AOS_VECTOR_ARG
+		#define SCE_VECTORMATH_AOS_MATRIX_ARG SCE_VECTORMATH_SCALAR_AOS_MATRIX_ARG
+
+		namespace sce {
+		namespace PhysicsEffects {
+		typedef sce::Vectormath::Scalar::Aos::Point3     PfxPoint3;
+		typedef sce::Vectormath::Scalar::Aos::Vector3    PfxVector3;
+		typedef sce::Vectormath::Scalar::Aos::Vector4    PfxVector4;
+		typedef sce::Vectormath::Scalar::Aos::Quat       PfxQuat;
+		typedef sce::Vectormath::Scalar::Aos::Matrix3    PfxMatrix3;
+		typedef sce::Vectormath::Scalar::Aos::Matrix4    PfxMatrix4;
+		typedef sce::Vectormath::Scalar::Aos::Transform3 PfxTransform3;
+		typedef sce::Vectormath::Scalar::floatInVec		PfxFloatInVec;
+		typedef sce::Vectormath::Scalar::boolInVec		PfxBoolInVec;
+		} //namespace PhysicsEffects
+		} //namespace sce
+	#endif
+#endif
+
+#ifdef SCE_PFX_USE_GEOMETRY
+#include <sce_geometry.h>
+namespace sce {
+namespace PhysicsEffects {
+typedef sce::Geometry::Aos::Line	PfxGeomLine;
+typedef sce::Geometry::Aos::Segment	PfxGeomSegment;
+typedef sce::Geometry::Aos::Plane	PfxGeomPlane;
+typedef sce::Geometry::Aos::Sphere	PfxGeomSphere;
+typedef sce::Geometry::Aos::Capsule	PfxGeomCapsule;
+typedef sce::Geometry::Aos::Bounds	PfxGeomBounds;
+typedef sce::Geometry::Aos::Aabb	PfxGeomAabb;
+typedef sce::Geometry::Aos::Obb		PfxGeomObb;
+} //namespace PhysicsEffects
+} //namespace sce
+#endif
+
+#endif // _SCE_PFX_VECTORMATH_INCLUDE_WIN32_H

Extras/PhysicsEffects/include/physics_effects/base_level/broadphase/pfx_broadphase_pair.h

@@ -0,0 +1,46 @@
+/*
+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_BROADPHASE_PAIR_H
+#define _SCE_PFX_BROADPHASE_PAIR_H
+
+#include "../sort/pfx_sort_data.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+typedef PfxSortData16 PfxBroadphasePair;
+
+SCE_PFX_FORCE_INLINE void pfxSetObjectIdA(PfxBroadphasePair &pair,PfxUInt16 i)	{pair.set16(0,i);}
+SCE_PFX_FORCE_INLINE void pfxSetObjectIdB(PfxBroadphasePair &pair,PfxUInt16 i)	{pair.set16(1,i);}
+SCE_PFX_FORCE_INLINE void pfxSetMotionMaskA(PfxBroadphasePair &pair,PfxUInt8 i)		{pair.set8(4,i);}
+SCE_PFX_FORCE_INLINE void pfxSetMotionMaskB(PfxBroadphasePair &pair,PfxUInt8 i)		{pair.set8(5,i);}
+SCE_PFX_FORCE_INLINE void pfxSetBroadphaseFlag(PfxBroadphasePair &pair,PfxUInt8 f)	{pair.set8(6,(pair.get8(6)&0xf0)|(f&0x0f));}
+SCE_PFX_FORCE_INLINE void pfxSetActive(PfxBroadphasePair &pair,PfxBool b)			{pair.set8(6,(pair.get8(6)&0x0f)|((b?1:0)<<4));}
+SCE_PFX_FORCE_INLINE void pfxSetContactId(PfxBroadphasePair &pair,PfxUInt32 i)		{pair.set32(2,i);}
+
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetObjectIdA(const PfxBroadphasePair &pair)	{return pair.get16(0);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetObjectIdB(const PfxBroadphasePair &pair)	{return pair.get16(1);}
+SCE_PFX_FORCE_INLINE PfxUInt8  pfxGetMotionMaskA(const PfxBroadphasePair &pair)		{return pair.get8(4);}
+SCE_PFX_FORCE_INLINE PfxUInt8  pfxGetMotionMaskB(const PfxBroadphasePair &pair)		{return pair.get8(5);}
+SCE_PFX_FORCE_INLINE PfxUInt8  pfxGetBroadphaseFlag(const PfxBroadphasePair &pair)	{return pair.get8(6)&0x0f;}
+SCE_PFX_FORCE_INLINE PfxBool   pfxGetActive(const PfxBroadphasePair &pair)			{return (pair.get8(6)>>4)!=0;}
+SCE_PFX_FORCE_INLINE PfxUInt32 pfxGetContactId(const PfxBroadphasePair &pair)		{return pair.get32(2);}
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_BROADPHASE_PAIR_H

Extras/PhysicsEffects/include/physics_effects/base_level/broadphase/pfx_broadphase_proxy.h

@@ -0,0 +1,42 @@
+/*
+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_BROADPHASE_PROXY_H
+#define _SCE_PFX_BROADPHASE_PROXY_H
+
+namespace sce {
+namespace PhysicsEffects {
+
+typedef PfxSortData32 PfxBroadphaseProxy;
+
+//J	AABBパラメータはPfxAabbと共通
+//E PfxBroadphaseProxy shares AABB parameters with PfxAabb32
+
+SCE_PFX_FORCE_INLINE void pfxSetObjectId(PfxBroadphaseProxy &proxy,PfxUInt16 i)	   {proxy.set16(6,i);}
+SCE_PFX_FORCE_INLINE void pfxSetMotionMask(PfxBroadphaseProxy &proxy,PfxUInt8 i)   {proxy.set8(14,i);}
+SCE_PFX_FORCE_INLINE void pfxSetProxyFlag(PfxBroadphaseProxy &proxy,PfxUInt8 i)    {proxy.set8(15,i);}
+SCE_PFX_FORCE_INLINE void pfxSetSelf(PfxBroadphaseProxy &proxy,PfxUInt32 i)        {proxy.set32(5,i);}
+SCE_PFX_FORCE_INLINE void pfxSetTarget(PfxBroadphaseProxy &proxy,PfxUInt32 i)      {proxy.set32(6,i);}
+
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetObjectId(const PfxBroadphaseProxy &proxy)     {return proxy.get16(6);}
+SCE_PFX_FORCE_INLINE PfxUInt8  pfxGetMotionMask(const PfxBroadphaseProxy &proxy)   {return proxy.get8(14);}
+SCE_PFX_FORCE_INLINE PfxUInt8  pfxGetProxyFlag(const PfxBroadphaseProxy &proxy)	   {return proxy.get8(15);}
+SCE_PFX_FORCE_INLINE PfxUInt32 pfxGetSelf(const PfxBroadphaseProxy &proxy)		   {return proxy.get32(5);}
+SCE_PFX_FORCE_INLINE PfxUInt32 pfxGetTarget(const PfxBroadphaseProxy &proxy)	   {return proxy.get32(6);}
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_BROADPHASE_PROXY_H

Extras/PhysicsEffects/include/physics_effects/base_level/broadphase/pfx_update_broadphase_proxy.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_UPDATE_BROADPHASE_PROXY_H
+#define _SCE_PFX_UPDATE_BROADPHASE_PROXY_H
+
+#include "pfx_broadphase_pair.h"
+#include "pfx_broadphase_proxy.h"
+#include "../rigidbody/pfx_rigid_state.h"
+#include "../collision/pfx_collidable.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Update Broadphase Proxy
+
+//E For single axis
+//J 単一軸に対して作成
+PfxInt32 pfxUpdateBroadphaseProxy(
+	PfxBroadphaseProxy &proxy,
+	const PfxRigidState &state,
+	const PfxCollidable &coll,
+	const PfxVector3 &worldCenter,
+	const PfxVector3 &worldExtent,
+	PfxUInt32 axis);
+
+PfxInt32 pfxUpdateBroadphaseProxy(
+	PfxBroadphaseProxy &proxy,
+	const PfxRigidState &state,
+	const PfxVector3 &objectCenter,
+	const PfxVector3 &objectHalf,
+	const PfxVector3 &worldCenter,
+	const PfxVector3 &worldExtent,
+	PfxUInt32 axis);
+
+//E For all axes
+//J 全軸に対して作成
+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 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);
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_UPDATE_BROADPHASE_PROXY_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_aabb.h

@@ -0,0 +1,124 @@
+/*
+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_AABB_H
+#define _SCE_PFX_AABB_H
+
+#include "../sort/pfx_sort_data.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+typedef PfxSortData16 PfxAabb16;
+typedef PfxSortData32 PfxAabb32;
+
+SCE_PFX_FORCE_INLINE void pfxSetXMin(PfxAabb16& aabb,PfxUInt16 i) {aabb.set16(0,i);}
+SCE_PFX_FORCE_INLINE void pfxSetXMax(PfxAabb16& aabb,PfxUInt16 i) {aabb.set16(1,i);}
+SCE_PFX_FORCE_INLINE void pfxSetYMin(PfxAabb16& aabb,PfxUInt16 i) {aabb.set16(2,i);}
+SCE_PFX_FORCE_INLINE void pfxSetYMax(PfxAabb16& aabb,PfxUInt16 i) {aabb.set16(3,i);}
+SCE_PFX_FORCE_INLINE void pfxSetZMin(PfxAabb16& aabb,PfxUInt16 i) {aabb.set16(4,i);}
+SCE_PFX_FORCE_INLINE void pfxSetZMax(PfxAabb16& aabb,PfxUInt16 i) {aabb.set16(5,i);}
+SCE_PFX_FORCE_INLINE void pfxSetXYZMin(PfxAabb16 &aabb,PfxUInt16 i,int axis) {aabb.set16(axis<<1,i);}
+SCE_PFX_FORCE_INLINE void pfxSetXYZMax(PfxAabb16 &aabb,PfxUInt16 i,int axis) {aabb.set16((axis<<1)+1,i);}
+
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXMin(const PfxAabb16& aabb) {return aabb.get16(0);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXMax(const PfxAabb16& aabb) {return aabb.get16(1);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetYMin(const PfxAabb16& aabb) {return aabb.get16(2);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetYMax(const PfxAabb16& aabb) {return aabb.get16(3);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetZMin(const PfxAabb16& aabb) {return aabb.get16(4);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetZMax(const PfxAabb16& aabb) {return aabb.get16(5);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXYZMin(const PfxAabb16 &aabb,int axis) {return aabb.get16(axis<<1);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXYZMax(const PfxAabb16 &aabb,int axis) {return aabb.get16((axis<<1)+1);}
+
+SCE_PFX_FORCE_INLINE void pfxSetXMin(PfxAabb32& aabb,PfxUInt16 i) {aabb.set16(0,i);}
+SCE_PFX_FORCE_INLINE void pfxSetXMax(PfxAabb32& aabb,PfxUInt16 i) {aabb.set16(1,i);}
+SCE_PFX_FORCE_INLINE void pfxSetYMin(PfxAabb32& aabb,PfxUInt16 i) {aabb.set16(2,i);}
+SCE_PFX_FORCE_INLINE void pfxSetYMax(PfxAabb32& aabb,PfxUInt16 i) {aabb.set16(3,i);}
+SCE_PFX_FORCE_INLINE void pfxSetZMin(PfxAabb32& aabb,PfxUInt16 i) {aabb.set16(4,i);}
+SCE_PFX_FORCE_INLINE void pfxSetZMax(PfxAabb32& aabb,PfxUInt16 i) {aabb.set16(5,i);}
+SCE_PFX_FORCE_INLINE void pfxSetXYZMin(PfxAabb32 &aabb,PfxUInt16 i,int axis) {aabb.set16(axis<<1,i);}
+SCE_PFX_FORCE_INLINE void pfxSetXYZMax(PfxAabb32 &aabb,PfxUInt16 i,int axis) {aabb.set16((axis<<1)+1,i);}
+
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXMin(const PfxAabb32& aabb) {return aabb.get16(0);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXMax(const PfxAabb32& aabb) {return aabb.get16(1);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetYMin(const PfxAabb32& aabb) {return aabb.get16(2);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetYMax(const PfxAabb32& aabb) {return aabb.get16(3);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetZMin(const PfxAabb32& aabb) {return aabb.get16(4);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetZMax(const PfxAabb32& aabb) {return aabb.get16(5);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXYZMin(const PfxAabb32 &aabb,int axis) {return aabb.get16(axis<<1);}
+SCE_PFX_FORCE_INLINE PfxUInt16 pfxGetXYZMax(const PfxAabb32 &aabb,int axis) {return aabb.get16((axis<<1)+1);}
+
+
+#define SCE_PFX_TEST_AABB(aabbA,aabbB) \
+if(pfxGetXMax(aabbA) < pfxGetXMin(aabbB) || pfxGetXMin(aabbA) > pfxGetXMax(aabbB)) return false;\
+if(pfxGetYMax(aabbA) < pfxGetYMin(aabbB) || pfxGetYMin(aabbA) > pfxGetYMax(aabbB)) return false;\
+if(pfxGetZMax(aabbA) < pfxGetZMin(aabbB) || pfxGetZMin(aabbA) > pfxGetZMax(aabbB)) return false;\
+return true;
+
+SCE_PFX_FORCE_INLINE
+bool pfxTestAabb(const PfxAabb16 &aabbA,const PfxAabb16 &aabbB)
+{
+SCE_PFX_TEST_AABB(aabbA,aabbB)
+}
+
+SCE_PFX_FORCE_INLINE
+bool pfxTestAabb(const PfxAabb32 &aabbA,const PfxAabb32 &aabbB)
+{
+SCE_PFX_TEST_AABB(aabbA,aabbB)
+}
+
+SCE_PFX_FORCE_INLINE
+bool pfxTestAabb(const PfxAabb32 &aabbA,const PfxAabb16 &aabbB)
+{
+SCE_PFX_TEST_AABB(aabbA,aabbB)
+}
+
+SCE_PFX_FORCE_INLINE
+bool pfxTestAabb(const PfxAabb16 &aabbA,const PfxAabb32 &aabbB)
+{
+SCE_PFX_TEST_AABB(aabbA,aabbB)
+}
+
+
+SCE_PFX_FORCE_INLINE
+PfxAabb16 pfxMergeAabb(const PfxAabb16 &aabbA,const PfxAabb16 &aabbB)
+{
+	PfxAabb16 aabb = aabbA;
+	pfxSetXMin(aabb,SCE_PFX_MIN(pfxGetXMin(aabbA),pfxGetXMin(aabbB)));
+	pfxSetXMax(aabb,SCE_PFX_MAX(pfxGetXMax(aabbA),pfxGetXMax(aabbB)));
+	pfxSetYMin(aabb,SCE_PFX_MIN(pfxGetYMin(aabbA),pfxGetYMin(aabbB)));
+	pfxSetYMax(aabb,SCE_PFX_MAX(pfxGetYMax(aabbA),pfxGetYMax(aabbB)));
+	pfxSetZMin(aabb,SCE_PFX_MIN(pfxGetZMin(aabbA),pfxGetZMin(aabbB)));
+	pfxSetZMax(aabb,SCE_PFX_MAX(pfxGetZMax(aabbA),pfxGetZMax(aabbB)));
+	return aabb;
+}
+
+SCE_PFX_FORCE_INLINE
+PfxAabb32 pfxMergeAabb(const PfxAabb32 &aabbA,const PfxAabb32 &aabbB)
+{
+	PfxAabb32 aabb = aabbA;
+	pfxSetXMin(aabb,SCE_PFX_MIN(pfxGetXMin(aabbA),pfxGetXMin(aabbB)));
+	pfxSetXMax(aabb,SCE_PFX_MAX(pfxGetXMax(aabbA),pfxGetXMax(aabbB)));
+	pfxSetYMin(aabb,SCE_PFX_MIN(pfxGetYMin(aabbA),pfxGetYMin(aabbB)));
+	pfxSetYMax(aabb,SCE_PFX_MAX(pfxGetYMax(aabbA),pfxGetYMax(aabbB)));
+	pfxSetZMin(aabb,SCE_PFX_MIN(pfxGetZMin(aabbA),pfxGetZMin(aabbB)));
+	pfxSetZMax(aabb,SCE_PFX_MAX(pfxGetZMax(aabbA),pfxGetZMax(aabbB)));
+	return aabb;
+}
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_AABB_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_box.h

@@ -0,0 +1,61 @@
+/*
+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_BOX_H
+#define _SCE_PFX_BOX_H
+
+#include "../base/pfx_common.h"
+
+namespace sce{
+namespace PhysicsEffects{
+struct PfxBox {
+	PfxVector3 m_half;
+
+	inline PfxBox() {}
+	inline PfxBox(const PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG half);
+	inline PfxBox(PfxFloat hx, PfxFloat hy, PfxFloat hz);
+
+	inline void set(const PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG half);
+	inline void set(PfxFloat hx, PfxFloat hy, PfxFloat hz);
+};
+
+inline
+PfxBox::PfxBox(const PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG half)
+{
+	set(half);
+}
+
+inline
+PfxBox::PfxBox(PfxFloat hx, PfxFloat hy, PfxFloat hz)
+{
+	set(hx, hy, hz);
+}
+
+inline
+void PfxBox::set(const PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG half)
+{
+	m_half = half;
+}
+
+inline
+void PfxBox::set(PfxFloat hx, PfxFloat hy, PfxFloat hz)
+{
+	m_half = PfxVector3(hx, hy, hz);
+}
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_BOX_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_capsule.h

@@ -0,0 +1,53 @@
+/*
+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_CAPSULE_H
+#define _SCE_PFX_CAPSULE_H
+
+#include "../base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxCapsule
+{
+	PfxFloat m_halfLen;
+	PfxFloat m_radius;
+
+	PfxCapsule() {}
+	PfxCapsule(PfxFloat halfLength, PfxFloat radius);
+
+	void set(PfxFloat halfLength, PfxFloat radius);
+};
+
+inline
+PfxCapsule::PfxCapsule(PfxFloat halfLength, PfxFloat radius)
+{
+	m_halfLen = halfLength;
+	m_radius = radius;
+}
+
+inline
+void PfxCapsule::set(PfxFloat halfLength, PfxFloat radius)
+{
+	m_halfLen = halfLength;
+	m_radius = radius;
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_CAPSULE_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_collidable.h

@@ -0,0 +1,74 @@
+/*
+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_COLLIDABLE_H
+#define _SCE_PFX_COLLIDABLE_H
+
+#include "pfx_shape.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_NUMPRIMS 64
+
+///////////////////////////////////////////////////////////////////////////////
+// Collidable Object
+
+class SCE_PFX_ALIGNED(128) PfxCollidable
+{
+friend class PfxShapeIterator;
+
+private:
+	PfxShape *m_shapeBase;
+	PfxUInt16 m_shapeIds[SCE_PFX_NUMPRIMS];
+	PfxUInt8 m_numShapes;
+	PfxUInt8 m_maxShapes;
+	SCE_PFX_PADDING(1,2)
+	PfxFloat m_center[3];	// AABB center (Local)
+	PfxFloat m_half[3];		// AABB half (Local)
+	PfxShape m_defShape;
+	SCE_PFX_PADDING(2,32)
+
+	inline PfxShape &getNewShape();
+
+public:
+	inline void reset();
+	inline void reset(PfxShape *base,PfxUInt16 *ids,int n=1);
+
+	void finish();
+
+	void addShape(const PfxShape &shape);
+
+	inline PfxUInt32 getNumShapes() const;
+	const PfxShape& getDefShape() const {return m_defShape;}
+	PfxShape& getDefShape() {return m_defShape;}
+
+	inline PfxUInt16 getShapeId(int i) const;
+
+	inline const PfxShape& getShape(int i) const;
+	inline PfxShape& getShape(int i);
+
+	inline PfxVector3 getHalf() const;
+	inline PfxVector3 getCenter() const;
+};
+
+#include "pfx_collidable_implementation.h"
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_COLLIDABLE_H
+

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_collidable_implementation.h

@@ -0,0 +1,114 @@
+/*
+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_COLLIDABLE_IMPLEMENTATION_H
+#define _SCE_PFX_COLLIDABLE_IMPLEMENTATION_H
+
+inline
+void PfxCollidable::reset()
+{
+	m_shapeBase = NULL;
+	m_numShapes = 0;
+	m_maxShapes = 1;
+	m_center[0] = 0.0f;
+	m_center[1] = 0.0f;
+	m_center[2] = 0.0f;
+	m_half[0] = 0.0f;
+	m_half[1] = 0.0f;
+	m_half[2] = 0.0f;
+}
+
+inline
+void PfxCollidable::reset(PfxShape *base,PfxUInt16 *ids,int n)
+{
+	m_shapeBase = base;
+	m_numShapes = 0;
+	m_maxShapes = n;
+	for(int i=0;i<n;i++) {
+		m_shapeIds[i] = ids[i];
+	}
+	m_center[0] = 0.0f;
+	m_center[1] = 0.0f;
+	m_center[2] = 0.0f;
+	m_half[0] = 0.0f;
+	m_half[1] = 0.0f;
+	m_half[2] = 0.0f;
+}
+
+inline
+PfxUInt32 PfxCollidable::getNumShapes() const
+{
+	return m_numShapes;
+}
+
+inline
+PfxUInt16 PfxCollidable::getShapeId(int i) const
+{
+	SCE_PFX_ASSERT(i>0);
+	return m_shapeIds[i-1];
+}
+
+inline
+const PfxShape& PfxCollidable::getShape(int i) const
+{
+	SCE_PFX_ASSERT(i<m_numShapes);
+	if(i>0) {
+		return m_shapeBase[m_shapeIds[i-1]];
+	}
+	else {
+		return m_defShape;
+	}
+}
+
+inline
+PfxShape& PfxCollidable::getShape(int i)
+{
+	SCE_PFX_ASSERT(i<m_numShapes);
+	if(i>0) {
+		return m_shapeBase[m_shapeIds[i-1]];
+	}
+	else {
+		return m_defShape;
+	}
+}
+
+inline
+PfxShape &PfxCollidable::getNewShape()
+{
+	SCE_PFX_ASSERT(m_numShapes<=m_maxShapes);
+	if(m_numShapes == 0) {
+		m_numShapes++;
+		return m_defShape;
+	}
+	else {
+		m_numShapes++;
+		return m_shapeBase[m_shapeIds[m_numShapes-2]];
+	}
+}
+
+inline
+PfxVector3 PfxCollidable::getHalf() const
+{
+	return pfxReadVector3(m_half);
+}
+
+inline
+PfxVector3 PfxCollidable::getCenter() const
+{
+	return pfxReadVector3(m_center);
+}
+
+#endif // _SCE_PFX_COLLIDABLE_IMPLEMENTATION_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_contact_manifold.h

@@ -0,0 +1,135 @@
+/*
+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_MANIFOLD_H
+#define _SCE_PFX_CONTACT_MANIFOLD_H
+
+#include "../base/pfx_common.h"
+#include "../base/pfx_vec_utils.h"
+#include "pfx_sub_data.h"
+#include "../solver/pfx_constraint_row.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_NUMCONTACTS_PER_BODIES 4
+
+///////////////////////////////////////////////////////////////////////////////
+// Contact Point
+
+struct PfxContactPoint
+{
+	PfxUInt8 m_duration;
+	PfxUInt8 m_shapeIdA;
+	PfxUInt8 m_shapeIdB;
+	SCE_PFX_PADDING(1,1)
+	PfxSubData m_subData;
+	PfxFloat m_distance;
+	PfxFloat m_localPointA[3];
+	PfxFloat m_localPointB[3];
+	SCE_PFX_PADDING(2,8)
+	PfxConstraintRow m_constraintRow[3];
+
+	void reset()
+	{
+		m_duration = 0;
+		m_shapeIdA = m_shapeIdB = 0;
+		m_subData = PfxSubData();
+		m_distance = SCE_PFX_FLT_MAX;
+		m_constraintRow[0].m_accumImpulse = 0.0f;
+		m_constraintRow[1].m_accumImpulse = 0.0f;
+		m_constraintRow[2].m_accumImpulse = 0.0f;
+	}
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Contact Manifold
+
+//J	同一ペアの衝突が続く限り保持されるコンタクト情報
+//E PfxContactManifold keeps contact information until two rigid bodies are separated.
+
+class SCE_PFX_ALIGNED(128) PfxContactManifold
+{
+private:
+	PfxUInt16 m_rigidBodyIdA,m_rigidBodyIdB;
+	PfxUInt16 m_duration;
+	PfxUInt16 m_numContacts;
+	PfxFloat  m_compositeFriction;
+	PfxUInt32 m_internalFlag;
+	PfxContactPoint m_contactPoints[SCE_PFX_NUMCONTACTS_PER_BODIES];
+	void		*m_userData;
+	PfxUInt32	m_userParam[4];
+	SCE_PFX_PADDING(1,28)
+
+	int findNearestContactPoint(const PfxPoint3 &newPoint,const PfxVector3 &newNormal);
+	int sort4ContactPoints(const PfxPoint3 &newPoint,PfxFloat newDistance);
+
+public:
+	// Internal method
+	PfxFloat getCompositeFriction() const {return m_compositeFriction;}
+	void setCompositeFriction(PfxFloat f) {m_compositeFriction = f;}
+
+	PfxUInt32 getInternalFlag() const {return m_internalFlag;}
+	void setInternalFlag(PfxUInt32 f) {m_internalFlag = f;}
+
+public:
+	void reset(PfxUInt16 rigidBodyIdA,PfxUInt16 rigidBodyIdB)
+	{
+		m_userData = 0;
+		m_userParam[0] = m_userParam[1] = m_userParam[2] = m_userParam[3] = 0;
+		m_numContacts = 0;
+		m_duration = 0;
+		m_rigidBodyIdA = rigidBodyIdA;
+		m_rigidBodyIdB = rigidBodyIdB;
+	}
+	
+	void addContactPoint(
+		PfxFloat newDistance,
+		const PfxVector3 &newNormal, // world coords
+		const PfxPoint3 &newPointA, // local coords to the objectA
+		const PfxPoint3 &newPointB, // local coords to the objectB
+		PfxSubData subData);
+	
+	void addContactPoint(const PfxContactPoint &cp);
+	
+	void removeContactPoint(int i)
+	{
+		SCE_PFX_ASSERT(i>=0&&i<m_numContacts);
+		m_contactPoints[i] = m_contactPoints[m_numContacts-1];
+		m_numContacts--;
+	}
+	
+	int getNumContacts() const {return (int)m_numContacts;}
+	
+	PfxContactPoint &getContactPoint(int i) {return m_contactPoints[i];}
+	
+	const PfxContactPoint &getContactPoint(int i) const {return m_contactPoints[i];}
+	
+	void refresh(const PfxVector3 &pA,const PfxQuat &qA,const PfxVector3 &pB,const PfxQuat &qB);
+	
+	void merge(const PfxContactManifold &contact);
+	
+	PfxUInt16 getDuration() const {return m_duration;}
+	
+	PfxUInt16 getRigidBodyIdA() const {return m_rigidBodyIdA;}
+	
+	PfxUInt16 getRigidBodyIdB() const {return m_rigidBodyIdB;}
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_CONTACT_MANIFOLD_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_cylinder.h

@@ -0,0 +1,53 @@
+/*
+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_CYLINDER_H
+#define _SCE_PFX_CYLINDER_H
+
+#include "../base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxCylinder
+{
+	PfxFloat m_halfLen;
+	PfxFloat m_radius;
+
+	PfxCylinder() {}
+	PfxCylinder(PfxFloat halfLength, PfxFloat radius);
+
+	void set(PfxFloat halfLength, PfxFloat radius);
+};
+
+inline
+PfxCylinder::PfxCylinder(PfxFloat halfLength, PfxFloat radius)
+{
+	m_halfLen = halfLength;
+	m_radius = radius;
+}
+
+inline
+void PfxCylinder::set(PfxFloat halfLength, PfxFloat radius)
+{
+	m_halfLen = halfLength;
+	m_radius = radius;
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_CYLINDER_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_large_tri_mesh.h

@@ -0,0 +1,122 @@
+/*
+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_LARGE_TRI_MESH_H
+#define _SCE_PFX_LARGE_TRI_MESH_H
+
+#include "pfx_tri_mesh.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_MAX_LARGETRIMESH_ISLANDS 256
+
+///////////////////////////////////////////////////////////////////////////////
+// Large Mesh
+
+class SCE_PFX_ALIGNED(16) PfxLargeTriMesh
+{
+public:
+	//J ラージメッシュのサイズ
+	//E Size of a large mesh
+	PfxVector3 m_half;
+
+	//J 含まれるメッシュの総数
+	//E Number of mesh groups
+	PfxUInt16 m_numIslands;
+
+	//J アイランドAABB配列（ソート済み）
+	//E Array of island AABB (Sorted)
+	PfxUInt8 m_axis; // 0 = X , 1 = Y , 2 = Z
+	SCE_PFX_PADDING(1,1)
+	PfxAabb16 *m_aabbList;
+	SCE_PFX_PADDING(2,4)
+
+	//J アイランド配列
+	//E Array of island
+	PfxTriMesh *m_islands;
+
+	PfxLargeTriMesh()
+	{
+		m_numIslands = 0;
+		m_islands = NULL;
+		m_aabbList = NULL;
+	}
+	
+	inline bool testAABB(int islandId,const PfxVector3 &center,const PfxVector3 &half) const;
+	
+	//J ワールド座標値をラージメッシュローカルに変換する
+	//E Convert a position in the world coordinate into a position in the local coordinate
+	inline PfxVecInt3 getLocalPosition(const PfxVector3 &worldPosition) const;
+	inline void getLocalPosition(
+		const PfxVector3 &worldMinPosition,const PfxVector3 &worldMaxPosition,
+		PfxVecInt3 &localMinPosition,PfxVecInt3 &localMaxPosition) const;
+	
+	//J ラージメッシュローカル座標値をワールドに変換する
+	//E Convert a position in the local coordinate into a position in the world coordinate
+	inline PfxVector3 getWorldPosition(const PfxVecInt3 &localPosition) const;
+};
+
+inline
+bool PfxLargeTriMesh::testAABB(int islandId,const PfxVector3 &center,const PfxVector3 &half) const 
+{
+	PfxVecInt3 aabbMinL = getLocalPosition(center-half);
+	PfxVecInt3 aabbMaxL = getLocalPosition(center+half);
+	
+	if(aabbMaxL.getX() < pfxGetXMin(m_aabbList[islandId]) || aabbMinL.getX() > pfxGetXMax(m_aabbList[islandId])) return false;
+	if(aabbMaxL.getY() < pfxGetYMin(m_aabbList[islandId]) || aabbMinL.getY() > pfxGetYMax(m_aabbList[islandId])) return false;
+	if(aabbMaxL.getZ() < pfxGetZMin(m_aabbList[islandId]) || aabbMinL.getZ() > pfxGetZMax(m_aabbList[islandId])) return false;
+	
+	return true;
+}
+
+inline
+PfxVecInt3 PfxLargeTriMesh::getLocalPosition(const PfxVector3 &worldPosition) const 
+{
+	const PfxVector3 sz(65535.0f);
+	PfxVector3 tmp = divPerElem(worldPosition+m_half,2.0f*m_half);
+	tmp = mulPerElem(sz,minPerElem(maxPerElem(tmp,PfxVector3(0.0f)),PfxVector3(1.0f))); // clamp 0.0 - 1.0
+	return PfxVecInt3(tmp);
+}
+
+inline
+void PfxLargeTriMesh::getLocalPosition(
+		const PfxVector3 &worldMinPosition,const PfxVector3 &worldMaxPosition,
+		PfxVecInt3 &localMinPosition,PfxVecInt3 &localMaxPosition) const
+{
+	const PfxVector3 sz(65535.0f);
+	PfxVector3 qmin = divPerElem(worldMinPosition+m_half,2.0f*m_half);
+	qmin = mulPerElem(sz,minPerElem(maxPerElem(qmin,PfxVector3(0.0f)),PfxVector3(1.0f))); // clamp 0.0 - 1.0
+
+	PfxVector3 qmax = divPerElem(worldMaxPosition+m_half,2.0f*m_half);
+	qmax = mulPerElem(sz,minPerElem(maxPerElem(qmax,PfxVector3(0.0f)),PfxVector3(1.0f))); // clamp 0.0 - 1.0
+
+localMinPosition = PfxVecInt3(floorf(qmin[0]),floorf(qmin[1]),floorf(qmin[2]));
+localMaxPosition = PfxVecInt3(ceilf(qmax[0]),ceilf(qmax[1]),ceilf(qmax[2]));
+}
+
+inline
+PfxVector3 PfxLargeTriMesh::getWorldPosition(const PfxVecInt3 &localPosition) const 
+{
+	PfxVector3 sz(65535.0f),lp(localPosition);
+	PfxVector3 tmp = divPerElem(lp,sz);
+	return mulPerElem(tmp,2.0f*m_half) - m_half;
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_LARGE_TRI_MESH_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_ray.h

@@ -0,0 +1,59 @@
+/*
+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_RAY_H
+#define _SCE_PFX_RAY_H
+
+#include "../base/pfx_common.h"
+#include "pfx_sub_data.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_RAY_FACET_MODE_FRONT_ONLY     0
+#define SCE_PFX_RAY_FACET_MODE_BACK_ONLY      1
+#define SCE_PFX_RAY_FACET_MODE_FRONT_AND_BACK 2
+
+struct SCE_PFX_ALIGNED(16) PfxRayInput
+{
+	PfxVector3 m_startPosition;
+	PfxVector3 m_direction;
+	PfxUInt32 m_contactFilterSelf;
+	PfxUInt32 m_contactFilterTarget;
+	PfxUInt8  m_facetMode;
+	SCE_PFX_PADDING(1,7)
+
+	void reset()
+	{
+		m_contactFilterSelf = m_contactFilterTarget = 0xffffffff;
+		m_facetMode = SCE_PFX_RAY_FACET_MODE_FRONT_ONLY;
+	}
+};
+
+struct SCE_PFX_ALIGNED(16) PfxRayOutput
+{
+	PfxVector3 m_contactPoint;
+	PfxVector3 m_contactNormal;
+	PfxFloat   m_variable;
+	PfxUInt16  m_objectId;
+	PfxUInt8   m_shapeId;
+	PfxBool    m_contactFlag : 1;
+	PfxSubData m_subData;
+};
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_RAY_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_shape.h

@@ -0,0 +1,118 @@
+/*
+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_SHAPE_H
+#define _SCE_PFX_SHAPE_H
+
+#include "../base/pfx_common.h"
+#include "../base/pfx_vec_utils.h"
+#include "pfx_box.h"
+#include "pfx_sphere.h"
+#include "pfx_capsule.h"
+#include "pfx_cylinder.h"
+#include "pfx_tri_mesh.h"
+#include "pfx_large_tri_mesh.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+enum ePfxShapeType
+{
+	kPfxShapeSphere = 0,
+	kPfxShapeBox,		
+	kPfxShapeCapsule,	
+	kPfxShapeCylinder,	
+	kPfxShapeConvexMesh,
+	kPfxShapeLargeTriMesh,
+	kPfxShapeReserved0,
+	kPfxShapeReserved1,
+	kPfxShapeReserved2,
+	kPfxShapeUser0,
+	kPfxShapeUser1,
+	kPfxShapeUser2,
+	kPfxShapeCount // =12
+};
+
+class SCE_PFX_ALIGNED(16) PfxShape
+{
+friend class PfxCollidable;
+
+private:
+	union {
+		PfxFloat  m_vecDataF[3];
+		PfxUInt32 m_vecDataI[3];
+		void	  *m_vecDataPtr[2];
+	};
+	PfxUInt8 m_type;
+	SCE_PFX_PADDING(1,3)
+	PfxFloat m_offsetPosition[3];
+	PfxFloat m_offsetOrientation[4];
+	PfxUInt32 m_contactFilterSelf;
+	PfxUInt32 m_contactFilterTarget;
+	SCE_PFX_PADDING(2,12)
+
+public:
+	inline void reset();
+	
+	// Shape
+	inline void setBox(PfxBox SCE_VECTORMATH_AOS_VECTOR_ARG box);
+	inline void setCapsule(PfxCapsule capsule);
+	inline void setCylinder(PfxCylinder cylinder);
+	inline void setSphere(PfxSphere sphere);
+	inline void setConvexMesh(const PfxConvexMesh *convexMesh);
+	inline void setLargeTriMesh(const PfxLargeTriMesh *largeMesh);
+
+	inline PfxUInt8			getType() const;
+	inline PfxBox			getBox()const ;
+	inline PfxCapsule		getCapsule() const;
+	inline PfxCylinder		getCylinder() const;
+	inline PfxSphere		getSphere() const;
+	inline const PfxConvexMesh*   getConvexMesh() const;
+	inline const PfxLargeTriMesh* getLargeTriMesh() const;
+
+	// Offset
+	inline void setOffsetTransform(const PfxTransform3 & xfrm);
+	inline void setOffsetOrientation(const PfxQuat SCE_VECTORMATH_AOS_VECTOR_ARG rot) {return pfxStoreQuat(rot,m_offsetOrientation);}
+	inline void setOffsetPosition(const PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG pos) {return pfxStoreVector3(pos,m_offsetPosition);}
+
+	inline PfxTransform3	getOffsetTransform() const;
+	inline PfxVector3		getOffsetPosition() const {return pfxReadVector3(m_offsetPosition);}
+	inline PfxQuat			getOffsetOrientation() const {return pfxReadQuat(m_offsetOrientation);}
+
+	// Raw data access
+	inline void setDataFloat(int i,PfxFloat v) {m_vecDataF[i]=v;}
+	inline void setDataInteger(int i,PfxUInt32 v) {m_vecDataI[i]=v;}
+
+	inline PfxFloat getDataFloat(int i) const {return m_vecDataF[i];}
+	inline PfxUInt32 getDataInteger(int i) const {return m_vecDataI[i];}
+
+	// Contact Filter
+	PfxUInt32	getContactFilterSelf() const {return m_contactFilterSelf;}
+	void		setContactFilterSelf(PfxUInt32 filter) {m_contactFilterSelf = filter;}
+
+	PfxUInt32	getContactFilterTarget() const {return m_contactFilterTarget;}
+	void		setContactFilterTarget(PfxUInt32 filter) {m_contactFilterTarget = filter;}
+	
+	// Bouding Volume
+	void getAabb(PfxVector3 &aabbMin,PfxVector3 &aabbMax) const;
+};
+
+#include "pfx_shape_implementation.h"
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_SHAPE_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_shape_implementation.h

@@ -0,0 +1,145 @@
+/*
+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_SHAPE_IMPLEMENTATION_H
+#define _SCE_PFX_SHAPE_IMPLEMENTATION_H
+
+inline
+void PfxShape::reset()
+{
+	m_type = kPfxShapeSphere;
+	m_offsetPosition[0] = m_offsetPosition[1] = m_offsetPosition[2]= 0.0f;
+	m_offsetOrientation[0] = m_offsetOrientation[1] = m_offsetOrientation[2]= 0.0f;
+	m_offsetOrientation[3] = 1.0f;
+	m_contactFilterSelf = m_contactFilterTarget = 0xffffffff;
+}
+
+inline
+void PfxShape::setBox(PfxBox SCE_VECTORMATH_AOS_VECTOR_ARG box)
+{
+	m_vecDataF[0] = box.m_half[0];
+	m_vecDataF[1] = box.m_half[1];
+	m_vecDataF[2] = box.m_half[2];
+	m_type = kPfxShapeBox;
+}
+
+inline
+void PfxShape::setCapsule(PfxCapsule capsule)
+{
+	m_vecDataF[0] = capsule.m_halfLen;
+	m_vecDataF[1] = capsule.m_radius;
+	m_vecDataF[2] = 0.0f;
+	m_type = kPfxShapeCapsule;
+}
+
+inline
+void PfxShape::setCylinder(PfxCylinder cylinder)
+{
+	m_vecDataF[0] = cylinder.m_halfLen;
+	m_vecDataF[1] = cylinder.m_radius;
+	m_vecDataF[2] = 0.0f;
+	m_type = kPfxShapeCylinder;
+}
+
+inline
+void PfxShape::setSphere(PfxSphere sphere)
+{
+	m_vecDataF[0] = sphere.m_radius;
+	m_vecDataF[1] = 0.0f;
+	m_vecDataF[2] = 0.0f;
+	m_type = kPfxShapeSphere;
+}
+
+inline
+void PfxShape::setConvexMesh(const PfxConvexMesh *convexMesh)
+{
+	m_vecDataPtr[0] = (void*)convexMesh;
+	m_vecDataPtr[1] = NULL;
+	m_type = kPfxShapeConvexMesh;
+}
+
+inline
+void PfxShape::setLargeTriMesh(const PfxLargeTriMesh *largeMesh)
+{
+	m_vecDataPtr[0] = (void*)largeMesh;
+	m_vecDataPtr[1] = NULL;
+	m_type = kPfxShapeLargeTriMesh;
+}
+
+inline
+void PfxShape::setOffsetTransform(const PfxTransform3 & xfrm)
+{
+	setOffsetOrientation(PfxQuat(xfrm.getUpper3x3()));
+	setOffsetPosition(xfrm.getTranslation());
+}
+
+inline
+PfxUInt8 PfxShape::getType() const
+{
+	return m_type;
+}
+
+inline
+PfxBox PfxShape::getBox() const
+{
+	SCE_PFX_ALWAYS_ASSERT(m_type==kPfxShapeBox);
+	return PfxBox(m_vecDataF[0],m_vecDataF[1],m_vecDataF[2]);
+}
+
+inline
+PfxCapsule PfxShape::getCapsule() const
+{
+	SCE_PFX_ALWAYS_ASSERT(m_type==kPfxShapeCapsule);
+	return PfxCapsule(m_vecDataF[0], m_vecDataF[1]);
+}
+
+inline
+PfxCylinder PfxShape::getCylinder() const
+{
+	SCE_PFX_ALWAYS_ASSERT(m_type==kPfxShapeCylinder);
+	return PfxCylinder(m_vecDataF[0], m_vecDataF[1]);
+}
+
+inline
+PfxSphere PfxShape::getSphere() const
+{
+	SCE_PFX_ALWAYS_ASSERT(m_type==kPfxShapeSphere);
+	return PfxSphere(m_vecDataF[0]);
+}
+
+inline
+const PfxConvexMesh *PfxShape::getConvexMesh() const
+{
+	SCE_PFX_ALWAYS_ASSERT(m_type==kPfxShapeConvexMesh);
+	SCE_PFX_ALWAYS_ASSERT(m_vecDataPtr[0]!=NULL);
+	return (PfxConvexMesh*)m_vecDataPtr[0];
+}
+
+inline
+const PfxLargeTriMesh *PfxShape::getLargeTriMesh() const
+{
+	SCE_PFX_ALWAYS_ASSERT(m_type==kPfxShapeLargeTriMesh);
+	SCE_PFX_ALWAYS_ASSERT(m_vecDataPtr[0]!=NULL);
+	return (PfxLargeTriMesh*)m_vecDataPtr[0];
+}
+
+inline
+PfxTransform3 PfxShape::getOffsetTransform() const
+{
+	return PfxTransform3(getOffsetOrientation(),getOffsetPosition());
+}
+
+#endif // _SCE_PFX_SHAPE_IMPLEMENTATION_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_shape_iterator.h

@@ -0,0 +1,58 @@
+/*
+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_SHAPE_ITERATOR_H
+#define _SCE_PFX_SHAPE_ITERATOR_H
+
+#include "pfx_collidable.h"
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+class PfxShapeIterator
+{
+private:
+PfxUInt32 m_numShapes;
+PfxShape *m_shapeBase;
+const PfxUInt16 *m_shapeIds;
+const PfxShape *m_curShape;
+PfxUInt32 m_index;
+
+public:
+PfxShapeIterator(const PfxCollidable &coll) : m_shapeIds(coll.m_shapeIds)
+{
+	m_numShapes = coll.m_numShapes;
+	m_shapeBase = coll.m_shapeBase;
+	m_index = 0;
+	m_curShape = &coll.m_defShape;
+}
+
+~PfxShapeIterator() {}
+
+inline PfxShapeIterator& operator++()
+{
+	m_curShape = &m_shapeBase[m_shapeIds[m_index++]];
+	return *this;
+}
+
+const PfxShape& operator*() const {return *m_curShape;}
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_SHAPE_ITERATOR_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_sphere.h

@@ -0,0 +1,49 @@
+/*
+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_SPHERE_H
+#define _SCE_PFX_SPHERE_H
+
+#include "../base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxSphere {
+	PfxFloat m_radius;
+
+	PfxSphere() {}
+	PfxSphere( PfxFloat radius );
+
+	void  set( PfxFloat radius );
+};
+
+inline
+PfxSphere::PfxSphere( PfxFloat radius )
+{
+	m_radius = radius;
+}
+
+inline
+void PfxSphere::set( PfxFloat radius )
+{
+	m_radius = radius;
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_SPHERE_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_sub_data.h

@@ -0,0 +1,63 @@
+/*
+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_SUB_DATA_H
+#define _SCE_PFX_SUB_DATA_H
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxSubData {
+	enum {
+		NONE = 0,
+		MESH_INFO
+	};
+
+	union {
+		struct {
+			PfxUInt8  m_type;
+			SCE_PFX_PADDING(1,1)
+
+			struct {
+				PfxUInt8  islandId;
+				PfxUInt8  facetId;
+				PfxUInt16 s;
+				PfxUInt16 t;
+
+			} m_facetLocal;
+		};
+		PfxUInt32 param[2];
+	};
+
+	PfxSubData()
+	{
+		param[0] = param[1] = 0;
+	}
+
+	void  setIslandId(PfxUInt8 i) {m_facetLocal.islandId = i;}
+	void  setFacetId(PfxUInt8 i) {m_facetLocal.facetId = i;}
+	void  setFacetLocalS(PfxFloat s) {m_facetLocal.s = (PfxUInt16)(s * 65535.0f);}
+	void  setFacetLocalT(PfxFloat t) {m_facetLocal.t = (PfxUInt16)(t * 65535.0f);}
+
+	PfxUInt8 getIslandId() {return m_facetLocal.islandId;}
+	PfxUInt8 getFacetId() {return m_facetLocal.facetId;}
+	PfxFloat getFacetLocalS() {return m_facetLocal.s / 65535.0f;}
+	PfxFloat getFacetLocalT() {return m_facetLocal.t / 65535.0f;}
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_SUB_DATA_H

Extras/PhysicsEffects/include/physics_effects/base_level/collision/pfx_tri_mesh.h

@@ -0,0 +1,154 @@
+/*
+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_TRI_MESH_H
+#define _SCE_PFX_TRI_MESH_H
+
+#include "../base/pfx_common.h"
+#include "../base/pfx_vec_utils.h"
+#include "pfx_aabb.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+//J メッシュのリソース制限
+//E Define some limitations of a triangle mesh
+#define SCE_PFX_NUMMESHFACETS		64
+#define SCE_PFX_NUMMESHEDGES		192
+#define SCE_PFX_NUMMESHVERTICES		128
+
+//J エッジの角
+//E Edge types
+#define SCE_PFX_EDGE_FLAT    0
+#define SCE_PFX_EDGE_CONVEX  1
+#define SCE_PFX_EDGE_CONCAVE 2
+
+///////////////////////////////////////////////////////////////////////////////
+// Edge
+struct PfxEdge
+{
+	PfxUInt8 m_vertId[2];
+	PfxUInt8 m_angleType;
+	PfxUInt8 m_tilt;
+};
+
+inline
+bool operator ==(const PfxEdge &e1,const PfxEdge &e2)
+{
+    return  (e1.m_vertId[0] == e2.m_vertId[0] && e1.m_vertId[1] == e2.m_vertId[1]) || 
+			(e1.m_vertId[1] == e2.m_vertId[0] && e1.m_vertId[0] == e2.m_vertId[1]);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Facet
+
+struct PfxFacet
+{
+	PfxFloat m_normal[3];
+	PfxFloat m_thickness;
+	PfxUInt8 m_group;
+	PfxUInt8 m_vertIds[3];
+	PfxUInt8 m_edgeIds[3];
+	PfxUInt8 m_userData;
+	PfxFloat m_half[3];
+	PfxFloat m_center[3];
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Mesh
+
+struct SCE_PFX_ALIGNED(16) PfxTriMesh
+{
+	PfxUInt8 m_numVerts;
+	PfxUInt8 m_numEdges;
+	PfxUInt8 m_numFacets;
+	PfxUInt8 m_reserved;
+	PfxFacet m_facets[SCE_PFX_NUMMESHFACETS];
+	PfxEdge  m_edges[SCE_PFX_NUMMESHEDGES];
+	SCE_PFX_PADDING(1,12)
+	PfxVector3 m_verts[SCE_PFX_NUMMESHVERTICES];
+	PfxVector3 m_half;
+	
+	PfxTriMesh()
+	{
+		m_numVerts = m_numEdges = m_numFacets = 0;
+	}
+	
+	inline void updateAABB();
+};
+
+inline
+void PfxTriMesh::updateAABB()
+{
+	PfxVector3 halfMax(0);
+	
+	for(PfxUInt8 i=0;i<m_numFacets;i++) {
+		PfxVector3 pnts[6] = {
+			m_verts[m_facets[i].m_vertIds[0]],
+			m_verts[m_facets[i].m_vertIds[1]],
+			m_verts[m_facets[i].m_vertIds[2]],
+			m_verts[m_facets[i].m_vertIds[0]] - m_facets[i].m_thickness * pfxReadVector3(m_facets[i].m_normal),
+			m_verts[m_facets[i].m_vertIds[1]] - m_facets[i].m_thickness * pfxReadVector3(m_facets[i].m_normal),
+			m_verts[m_facets[i].m_vertIds[2]] - m_facets[i].m_thickness * pfxReadVector3(m_facets[i].m_normal)
+		};
+		
+		PfxVector3 facetAABBmin,facetAABBmax,facetHalf,facetCenter;
+		facetAABBmin = minPerElem(pnts[5],minPerElem(pnts[4],minPerElem(pnts[3],minPerElem(pnts[2],minPerElem(pnts[0],pnts[1])))));
+		facetAABBmax = maxPerElem(pnts[5],maxPerElem(pnts[4],maxPerElem(pnts[3],maxPerElem(pnts[2],maxPerElem(pnts[0],pnts[1])))));
+		facetHalf = 0.5f * (facetAABBmax - facetAABBmin) + PfxVector3(0.00001f); // Slightly stretch to avoid collision hole
+		facetCenter = 0.5f * (facetAABBmax + facetAABBmin);
+		pfxStoreVector3(facetHalf,m_facets[i].m_half);
+		pfxStoreVector3(facetCenter,m_facets[i].m_center);
+		halfMax = maxPerElem(absPerElem(facetAABBmax),halfMax);
+	}
+	m_half = halfMax;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Convex Mesh
+
+struct SCE_PFX_ALIGNED(16) PfxConvexMesh
+{
+	PfxUInt8 m_numVerts;
+	PfxUInt8 m_numIndices;
+	PfxUInt16 m_indices[SCE_PFX_NUMMESHFACETS*3];
+	SCE_PFX_PADDING(1,14)
+	PfxVector3 m_verts[SCE_PFX_NUMMESHVERTICES];
+	PfxVector3 m_half;
+	SCE_PFX_PADDING(2,16)
+
+	PfxConvexMesh()
+	{
+		m_numVerts = m_numIndices = 0;
+	}
+	
+	inline void updateAABB();
+};
+
+inline
+void PfxConvexMesh::updateAABB()
+{
+	PfxVector3 halfMax(0);
+	for(int i=0;i<m_numVerts;i++) {
+		halfMax = maxPerElem(absPerElem(m_verts[i]),halfMax);
+	}
+	m_half = halfMax;
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_TRI_MESH_H

Extras/PhysicsEffects/include/physics_effects/base_level/pfx_base_level_include.h

@@ -0,0 +1,50 @@
+/*
+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_BASE_LEVEL_INCLUDE_H
+#define _SCE_PFX_BASE_LEVEL_INCLUDE_H
+
+///////////////////////////////////////////////////////////////////////////////
+// Physics Effects Base Level Headers
+
+#include "base/pfx_common.h"
+#include "base/pfx_perf_counter.h"
+
+#include "rigidbody/pfx_rigid_state.h"
+#include "rigidbody/pfx_rigid_body.h"
+
+#include "broadphase/pfx_broadphase_pair.h"
+#include "broadphase/pfx_broadphase_proxy.h"
+#include "broadphase/pfx_update_broadphase_proxy.h"
+
+#include "collision/pfx_collidable.h"
+#include "collision/pfx_shape_iterator.h"
+#include "collision/pfx_contact_manifold.h"
+
+#include "solver/pfx_constraint_pair.h"
+#include "solver/pfx_contact_constraint.h"
+#include "solver/pfx_joint_ball.h"
+#include "solver/pfx_joint_swing_twist.h"
+#include "solver/pfx_joint_hinge.h"
+#include "solver/pfx_joint_slider.h"
+#include "solver/pfx_joint_fix.h"
+#include "solver/pfx_joint_universal.h"
+
+#include "solver/pfx_integrate.h"
+
+#include "sort/pfx_sort.h"
+
+#endif // _SCE_PFX_BASE_LEVEL_INCLUDE_H

Extras/PhysicsEffects/include/physics_effects/base_level/rigidbody/pfx_rigid_body.h

@@ -0,0 +1,71 @@
+/*
+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_RIGID_BODY_H
+#define _SCE_PFX_RIGID_BODY_H
+
+#include "../base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+class SCE_PFX_ALIGNED(128) PfxRigidBody
+{
+private:
+	// Rigid Body constants
+	PfxMatrix3     m_inertia;     // Inertia matrix in body's coords
+	PfxMatrix3     m_inertiaInv;  // Inertia matrix inverse in body's coords
+	PfxFloat       m_mass;        // Rigid Body mass
+	PfxFloat       m_massInv;     // Inverse of mass
+	PfxFloat       m_restitution; // Coefficient of restitution
+	PfxFloat       m_friction;    // Coefficient of friction
+	SCE_PFX_PADDING(1,16)
+	
+public:
+	PfxFloat       getMassInv() const {return m_massInv;}
+	void           setMassInv(PfxFloat massInv) {m_massInv=massInv;}
+
+	const PfxMatrix3& getInertiaInv() const {return m_inertiaInv;}
+	void              setInertiaInv(const PfxMatrix3 SCE_VECTORMATH_AOS_MATRIX_ARG inertiaInv) {m_inertiaInv = inertiaInv;}
+
+public:
+	inline void reset();
+
+	PfxFloat       getMass() const {return m_mass;};
+	void           setMass(PfxFloat mass) {m_mass=mass;m_massInv=mass>0.0f?1.0f/mass:0.0f;}
+
+	const PfxMatrix3& getInertia() const {return m_inertia;}
+	void              setInertia(const PfxMatrix3 SCE_VECTORMATH_AOS_MATRIX_ARG inertia) {m_inertia = inertia;m_inertiaInv = inverse(inertia);}
+
+	PfxFloat       getRestitution() const {return m_restitution;}
+	void           setRestitution(PfxFloat restitution) {m_restitution = restitution;}
+
+	PfxFloat       getFriction() const {return m_friction;}
+	void           setFriction(PfxFloat friction) {m_friction = friction;}
+};
+
+inline
+void PfxRigidBody::reset()
+{
+	m_mass = 0.0f;
+	m_restitution = 0.2f;
+	m_friction = 0.6f;
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_RIGID_BODY_H

Extras/PhysicsEffects/include/physics_effects/base_level/rigidbody/pfx_rigid_state.h

@@ -0,0 +1,196 @@
+/*
+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_RIGID_STATE_H
+#define _SCE_PFX_RIGID_STATE_H
+
+#include "../base/pfx_common.h"
+#include "../base/pfx_vec_utils.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+// Motion Type
+enum ePfxMotionType {
+	kPfxMotionTypeFixed = 0,
+	kPfxMotionTypeActive,
+	kPfxMotionTypeKeyframe,
+	kPfxMotionTypeOneWay,
+	kPfxMotionTypeTrigger,
+	kPfxMotionTypeCount
+};
+
+#define SCE_PFX_MOTION_MASK_DYNAMIC(motion)   ((1<<(motion))&0x0a) // Active,OneWay
+#define SCE_PFX_MOTION_MASK_STATIC(motion)    ((1<<(motion))&0x95) // Fixed,Keyframe
+#define SCE_PFX_MOTION_MASK_CAN_SLEEP(motion) ((1<<(motion))&0x0e) // Can sleep
+
+#define SCE_PFX_MOTION_MASK_SLEEPING 0x80 // Is sleeping
+#define SCE_PFX_MOTION_MASK_TYPE     0x7f // Motion Type
+
+class SCE_PFX_ALIGNED(128) PfxRigidState
+{
+private:
+	union {
+		struct {
+			PfxUInt8	m_useSleep  : 1;
+			PfxUInt8	m_sleeping  : 1;
+			PfxUInt8	m_reserved1 : 1;
+			PfxUInt8	m_reserved2 : 1;
+			PfxUInt8	m_reserved3 : 1;
+			PfxUInt8	m_reserved4 : 1;
+			PfxUInt8	m_reserved5 : 1;
+			PfxUInt8	m_reserved6 : 1;
+		};
+		PfxUInt8 m_flags;
+	};
+	PfxUInt8	m_motionType;
+	PfxUInt16	m_sleepCount;
+	PfxUInt16	m_rigidBodyId;
+
+	SCE_PFX_PADDING(1,2)
+
+	PfxUInt32	m_contactFilterSelf;
+	PfxUInt32	m_contactFilterTarget;
+
+	PfxFloat	m_linearDamping;
+	PfxFloat	m_angularDamping;
+
+	PfxFloat	m_maxLinearVelocity;
+	PfxFloat	m_maxAngularVelocity;
+	
+	PfxVector3	m_position;
+	PfxQuat		m_orientation;
+	PfxVector3	m_linearVelocity;
+	PfxVector3	m_angularVelocity;
+
+	void		*m_userData;
+	PfxUInt32	m_userParam[4];
+	
+	SCE_PFX_PADDING(2,12)
+
+public:
+	inline void reset();
+
+	PfxUInt16	getRigidBodyId() const {return m_rigidBodyId;}
+	void		setRigidBodyId(PfxUInt16 i) {m_rigidBodyId = i;}
+
+	PfxUInt32	getContactFilterSelf() const {return m_contactFilterSelf;}
+	void		setContactFilterSelf(PfxUInt32 filter) {m_contactFilterSelf = filter;}
+
+	PfxUInt32	getContactFilterTarget() const {return m_contactFilterTarget;}
+	void		setContactFilterTarget(PfxUInt32 filter) {m_contactFilterTarget = filter;}
+
+	ePfxMotionType	getMotionType() const {return (ePfxMotionType)m_motionType;}
+	void		setMotionType(ePfxMotionType t) {SCE_PFX_ALWAYS_ASSERT(t<kPfxMotionTypeCount);m_motionType = (PfxUInt8)t;wakeup();}
+
+	PfxUInt8	getMotionMask() const {return m_motionType|(m_sleeping<<7);}
+
+	PfxBool		isAsleep() const {return m_sleeping==1;}
+	PfxBool		isAwake() const {return m_sleeping==0;}
+
+	void		wakeup() {m_sleeping=0;m_sleepCount=0;}
+	void		sleep() {if(m_useSleep) {m_sleeping=1;m_sleepCount=0;}}
+
+	PfxUInt8	getUseSleep() const {return m_useSleep;}
+	void		setUseSleep(PfxUInt8 b) {m_useSleep=b;}
+
+	void	 	incrementSleepCount() {if(m_sleepCount<0xffff)m_sleepCount++;}
+	void		resetSleepCount() {m_sleepCount=0;}
+	PfxUInt16	getSleepCount() const {return m_sleepCount;}
+
+	PfxFloat getLinearDamping() const {return m_linearDamping;}
+	PfxFloat getAngularDamping() const {return m_angularDamping;}
+
+	PfxFloat getMaxLinearVelocity() const {return m_maxLinearVelocity;}
+	PfxFloat getMaxAngularVelocity() const {return m_maxAngularVelocity;}
+
+	void setLinearDamping(PfxFloat damping) {m_linearDamping=damping;}
+	void setAngularDamping(PfxFloat damping) {m_angularDamping=damping;}
+
+	void setMaxLinearVelocity(PfxFloat maxvelocity){m_maxLinearVelocity = maxvelocity;}
+	void setMaxAngularVelocity(PfxFloat maxvelocity){m_maxAngularVelocity = maxvelocity;}
+
+	PfxVector3 getPosition() const {return m_position;}
+	PfxQuat    getOrientation() const {return m_orientation;}
+	PfxVector3 getLinearVelocity() const {return m_linearVelocity;}
+	PfxVector3 getAngularVelocity() const {return m_angularVelocity;}
+
+	void setPosition(const PfxVector3 &pos) {m_position = pos;}
+	void setOrientation(const PfxQuat &rot) {m_orientation = rot;}
+	void setLinearVelocity(const PfxVector3 &vel) {m_linearVelocity = vel;}
+	void setAngularVelocity(const PfxVector3 &vel) {m_angularVelocity = vel;}
+
+	void		*getUserData() const {return m_userData;}
+	void		setUserData(void *d) {m_userData=d;}
+
+	inline void movePosition(const PfxVector3 &pos,PfxFloat timeStep);
+	inline void moveOrientation(const PfxQuat &rot,PfxFloat timeStep);
+};
+
+inline
+void PfxRigidState::reset()
+{
+	m_sleepCount = 0;
+	m_motionType = kPfxMotionTypeActive;
+	m_flags = 0;
+	m_rigidBodyId = 0;
+	m_contactFilterSelf = 0xffffffff;
+	m_contactFilterTarget = 0xffffffff;
+	m_linearDamping = 1.0f;
+	m_angularDamping = 0.99f;
+	m_maxLinearVelocity = 200.0f;
+	m_maxAngularVelocity = SCE_PFX_PI * 100.0f;
+	m_position = PfxVector3(0.0f);
+	m_orientation = PfxQuat::identity();
+	m_linearVelocity = PfxVector3(0.0f);
+	m_angularVelocity = PfxVector3(0.0f);
+	m_userData = NULL;
+}
+
+inline
+void PfxRigidState::movePosition(const PfxVector3 &pos,PfxFloat timeStep)
+{
+	if(getMotionType()!=kPfxMotionTypeKeyframe) return;
+
+	SCE_PFX_ASSERT(timeStep > 0.0f);
+
+	setLinearVelocity((pos - getPosition()) / timeStep);
+}
+
+inline
+void PfxRigidState::moveOrientation(const PfxQuat &rot,PfxFloat timeStep)
+{
+	if(getMotionType()!=kPfxMotionTypeKeyframe) return;
+
+	SCE_PFX_ASSERT(timeStep > 0.0f);
+
+	PfxQuat ori1 = getOrientation();
+	PfxQuat ori2 = rot;
+
+	if(dot(ori2,ori1) < 0.0f) {
+		ori2 = -rot;
+	}
+
+	PfxQuat dq = ( ori2 - ori1 ) / timeStep;
+	dq = dq * 2.0f * conj(ori1);
+	PfxVector3 omega = dq.getXYZ();
+
+	setAngularVelocity(omega);
+}
+} // namespace PhysicsEffects
+} // namespace sce
+#endif // _SCE_PFX_RIGID_STATE_H
+

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_constraint_pair.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_CONSTRAINT_PAIR_H
+#define _SCE_PFX_CONSTRAINT_PAIR_H
+
+#include "../sort/pfx_sort_data.h"
+#include "../broadphase/pfx_broadphase_pair.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+typedef PfxSortData16 PfxConstraintPair;
+
+//J	PfxBroadphasePairと共通
+//E Same as PfxBroadphasePair
+
+SCE_PFX_FORCE_INLINE void pfxSetConstraintId(PfxConstraintPair &pair,PfxUInt32 i)	{pair.set32(2,i);}
+SCE_PFX_FORCE_INLINE void pfxSetNumConstraints(PfxConstraintPair &pair,PfxUInt8 n)	{pair.set8(7,n);}
+
+SCE_PFX_FORCE_INLINE PfxUInt32 pfxGetConstraintId(const PfxConstraintPair &pair)	{return pair.get32(2);}
+SCE_PFX_FORCE_INLINE PfxUInt8  pfxGetNumConstraints(const PfxConstraintPair &pair)	{return pair.get8(7);}
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_CONSTRAINT_PAIR_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_constraint_row.h

@@ -0,0 +1,40 @@
+/*
+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_H
+#define _SCE_PFX_CONSTRAINT_ROW_H
+
+#include "../base/pfx_vec_utils.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+
+#include "../base/pfx_common.h"
+
+//E Don't change following order of parameters
+struct SCE_PFX_ALIGNED(16) PfxConstraintRow {
+	PfxFloat m_normal[3];
+	PfxFloat m_rhs;
+	PfxFloat m_jacDiagInv;
+	PfxFloat m_lowerLimit;
+	PfxFloat m_upperLimit;
+	PfxFloat m_accumImpulse;
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_CONSTRAINT_ROW_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_contact_constraint.h

@@ -0,0 +1,59 @@
+/*
+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_CONSTRAINT_H
+#define _SCE_PFX_CONTACT_CONSTRAINT_H
+
+#include "../rigidbody/pfx_rigid_state.h"
+#include "pfx_constraint_row.h"
+#include "pfx_solver_body.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+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 pfxSolveContactConstraint(
+	PfxConstraintRow &constraintResponse,
+	PfxConstraintRow &constraintFriction1,
+	PfxConstraintRow &constraintFriction2,
+	const PfxVector3 &contactPointA,
+	const PfxVector3 &contactPointB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat friction
+	);
+
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_CONTACT_CONSTRAINT_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_integrate.h

@@ -0,0 +1,121 @@
+/*
+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_INTEGRATE_H_
+#define _SCE_PFX_INTEGRATE_H_
+
+#include "../base/pfx_common.h"
+#include "../rigidbody/pfx_rigid_body.h"
+#include "../rigidbody/pfx_rigid_state.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+template <typename T>
+static SCE_PFX_FORCE_INLINE T pfxRungeKutta(const T &deriv,PfxFloat dt)
+{
+	T	k0, k1, k2, k3;
+	k0 = deriv * dt;
+	k1 = (deriv + k0 * 0.5f) * dt;
+	k2 = (deriv + k1 * 0.5f) * dt;
+	k3 = (deriv + k2) * dt;
+	return (k0 + k1*2.0f + k2*2.0f + k3) / 6.0f;
+}
+
+#define SCE_PFX_MOTION_MASK_INTEGRATE ((1<<kPfxMotionTypeFixed)|(1<<kPfxMotionTypeTrigger))
+
+static SCE_PFX_FORCE_INLINE
+void pfxIntegrate(
+	PfxRigidState &state,
+	const PfxRigidBody &body,
+	PfxFloat timeStep
+	)
+{
+	(void) body;
+	if(((1<<state.getMotionMask())&SCE_PFX_MOTION_MASK_INTEGRATE) || state.isAsleep()) return;
+
+	PfxVector3 position = state.getPosition();
+	PfxQuat    orientation = state.getOrientation();
+	PfxVector3 linearVelocity = state.getLinearVelocity();
+	PfxVector3 angularVelocity = state.getAngularVelocity();
+
+	PfxVector3 dx = linearVelocity;
+	PfxQuat    dq = PfxQuat(angularVelocity,0) * orientation * 0.5f;
+
+#ifdef SCE_PFX_ODE_EULER
+	position += dx * timeStep;
+	orientation += dq * timeStep;
+	orientation = normalize(orientation);
+#else
+	position += pfxRungeKutta(dx,timeStep);
+	orientation += pfxRungeKutta(dq,timeStep);
+	orientation = normalize(orientation);
+#endif
+
+	state.setPosition(position);
+	state.setOrientation(orientation);
+}
+
+#define SCE_PFX_MOTION_MASK_APPLYFORCE ((1<<kPfxMotionTypeFixed)|(1<<kPfxMotionTypeTrigger)|(1<<kPfxMotionTypeKeyframe))
+
+static SCE_PFX_FORCE_INLINE
+void pfxApplyExternalForce(
+	PfxRigidState &state,
+	const PfxRigidBody &body,
+	const PfxVector3 &extForce,
+	const PfxVector3 &extTorque,
+	PfxFloat timeStep
+	)
+{
+	if(((1<<state.getMotionType())&SCE_PFX_MOTION_MASK_APPLYFORCE) || state.isAsleep()) return;
+	
+	PfxVector3 angularVelocity = state.getAngularVelocity();
+	PfxMatrix3 m(state.getOrientation());
+	PfxMatrix3 worldInertia = m * body.getInertia() * transpose(m);
+	PfxMatrix3 worldInertiaInv = m * body.getInertiaInv() * transpose(m);
+	PfxVector3 dv = extForce * body.getMassInv();
+	PfxVector3 dw = worldInertiaInv * (cross(worldInertia * angularVelocity, angularVelocity) + extTorque);
+	
+	PfxVector3 nv = state.getLinearVelocity();
+	PfxVector3 nw = state.getAngularVelocity();
+
+#ifdef SCE_PFX_ODE_EULER
+	nv += dv * timeStep;
+	nw += dw * timeStep;
+#else
+	nv += pfxRungeKutta(dv,timeStep);
+	nw += pfxRungeKutta(dw,timeStep);
+#endif
+
+	nv *= state.getLinearDamping();
+	nw *= state.getAngularDamping();
+
+	if(length(nv) > state.getMaxLinearVelocity())
+	{
+		nv = normalize( nv ) * state.getMaxLinearVelocity();
+	}
+	if(length(nw) > state.getMaxAngularVelocity())
+	{
+		nw = normalize( nw ) * state.getMaxAngularVelocity();
+	}
+
+	state.setLinearVelocity(nv);
+	state.setAngularVelocity(nw);
+}
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif /* _SCE_PFX_INTEGRATE_H_ */

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint.h

@@ -0,0 +1,90 @@
+/*
+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_JOINT_H
+#define _SCE_PFX_JOINT_H
+
+#include "../rigidbody/pfx_rigid_state.h"
+#include "pfx_joint_constraint.h"
+#include "pfx_constraint_pair.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+// Joint Type
+enum ePfxJointType {
+	kPfxJointBall = 0,
+	kPfxJointSwingtwist,
+	kPfxJointHinge,
+	kPfxJointSlider,
+	kPfxJointFix,
+	kPfxJointUniversal,
+	kPfxJointAnimation,
+	kPfxJointReserved0,
+	kPfxJointReserved1,
+	kPfxJointReserved2,
+	kPfxJointUser0,
+	kPfxJointUser1,
+	kPfxJointUser2,
+	kPfxJointUser3,
+	kPfxJointUser4,
+	kPfxJointCount // = 15
+};
+
+// Joint Structure
+struct SCE_PFX_ALIGNED(128) PfxJoint {
+	PfxUInt8 m_active;
+	PfxUInt8 m_numConstraints;
+	PfxUInt8 m_type;
+	SCE_PFX_PADDING(1,1)
+	PfxUInt16 m_rigidBodyIdA;
+	PfxUInt16 m_rigidBodyIdB;
+	SCE_PFX_PADDING(2,8)
+	PfxJointConstraint m_constraints[6];
+	void *m_userData;
+	SCE_PFX_PADDING(3,12)
+	PfxVector3 m_param[4]; // Used by some joints for specific features
+	PfxVector3 m_anchorA;
+	PfxVector3 m_anchorB;
+	PfxMatrix3 m_frameA;
+	PfxMatrix3 m_frameB;
+	SCE_PFX_PADDING(4,32)
+};
+
+SCE_PFX_FORCE_INLINE 
+void pfxUpdateJointPairs(
+	PfxConstraintPair &pair,
+	PfxUInt32 jointId,
+	const PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB
+	)
+{
+	SCE_PFX_ALWAYS_ASSERT(stateA.getRigidBodyId()==joint.m_rigidBodyIdA);
+	SCE_PFX_ALWAYS_ASSERT(stateB.getRigidBodyId()==joint.m_rigidBodyIdB);
+	pfxSetObjectIdA(pair,stateA.getRigidBodyId());
+	pfxSetObjectIdB(pair,stateB.getRigidBodyId());
+	pfxSetMotionMaskA(pair,stateA.getMotionMask());
+	pfxSetMotionMaskB(pair,stateB.getMotionMask());
+	pfxSetConstraintId(pair,jointId);
+	pfxSetNumConstraints(pair,joint.m_numConstraints);
+	pfxSetActive(pair,joint.m_active>0);
+}
+} //namespace PhysicsEffects
+} //namespace sce
+
+
+#endif // _PFX_JOINT_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint_ball.h

@@ -0,0 +1,62 @@
+/*
+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_JOINT_BALL_H
+#define _SCE_PFX_JOINT_BALL_H
+
+#include "pfx_joint.h"
+#include "pfx_solver_body.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxBallJointInitParam {
+	PfxVector3 anchorPoint;
+	
+	PfxBallJointInitParam()
+	{
+		anchorPoint = PfxVector3(0.0f);
+	}
+};
+
+PfxInt32 pfxInitializeBallJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	const PfxBallJointInitParam &param);
+
+void pfxSetupBallJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep);
+
+void pfxWarmStartBallJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB);
+
+void pfxSolveBallJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_JOINT_BALL_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint_constraint.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_JOINT_CONSTRAINT_H
+#define _SCE_PFX_JOINT_CONSTRAINT_H
+
+#include "../base/pfx_common.h"
+#include "../base/pfx_vec_utils.h"
+#include "pfx_constraint_row.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+// Lock type
+#define SCE_PFX_JOINT_LOCK_FREE		0
+#define SCE_PFX_JOINT_LOCK_LIMIT	1
+#define SCE_PFX_JOINT_LOCK_FIX		2
+
+// Slop
+#define SCE_PFX_JOINT_LINEAR_SLOP	0.01f
+#define SCE_PFX_JOINT_ANGULAR_SLOP	0.01f
+
+///////////////////////////////////////////////////////////////////////////////// Joint Constraint
+
+struct PfxJointConstraint {
+	PfxInt8 m_lock;
+	PfxInt8 m_warmStarting;
+	SCE_PFX_PADDING(1,2)
+	PfxFloat m_movableLowerLimit;
+	PfxFloat m_movableUpperLimit;
+	PfxFloat m_bias;
+	PfxFloat m_weight;
+	PfxFloat m_damping;
+	PfxFloat m_maxImpulse;
+	SCE_PFX_PADDING(2,4)
+	PfxConstraintRow m_constraintRow;
+	
+	void reset()
+	{
+		m_lock = SCE_PFX_JOINT_LOCK_FIX;
+		m_warmStarting = 0;
+		m_movableLowerLimit = 0.0f;
+		m_movableUpperLimit = 0.0f;
+		m_bias = 0.2f;
+		m_weight = 1.0f;
+		m_damping = 0.0f;
+		m_maxImpulse = SCE_PFX_FLT_MAX;
+		memset(&m_constraintRow,0,sizeof(PfxConstraintRow));
+	}
+};
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_JOINT_CONSTRAINT_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint_fix.h

@@ -0,0 +1,49 @@
+/*
+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_JOINT_FIX_H
+#define _SCE_PFX_JOINT_FIX_H
+
+#include "pfx_joint.h"
+#include "pfx_solver_body.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxFixJointInitParam {
+	PfxVector3 anchorPoint;
+	
+	PfxFixJointInitParam()
+	{
+		anchorPoint = PfxVector3(0.0f);
+	}
+};
+
+PfxInt32 pfxInitializeFixJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	const PfxFixJointInitParam &param);
+
+// pfxSetupFixJoint = pfxSetupSwingTwistJoint
+
+// pfxWarmStartFixJoint = pfxWarmStartSwingTwistJoint
+
+// pfxSolveFixJoint = pfxSolveSwingTwistJoint
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_JOINT_FIX_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint_hinge.h

@@ -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
+*/
+
+#ifndef _SCE_PFX_JOINT_HINGE_H_
+#define _SCE_PFX_JOINT_HINGE_H_
+
+#include "pfx_joint.h"
+#include "pfx_solver_body.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxHingeJointInitParam {
+	PfxVector3 anchorPoint;
+	PfxVector3 axis;
+	PfxFloat lowerAngle;
+	PfxFloat upperAngle;
+	SCE_PFX_PADDING(1,8)
+	
+	PfxHingeJointInitParam()
+	{
+		anchorPoint = PfxVector3(0.0f);
+		axis = PfxVector3(1.0f,0.0f,0.0f);
+		lowerAngle = 0.0f;
+		upperAngle = 0.0f;
+	}
+};
+
+PfxInt32 pfxInitializeHingeJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	const PfxHingeJointInitParam &param);
+
+// pfxSetupHingeJoint = pfxSetupSwingTwistJoint
+
+// pfxWarmStartHingeJoint = pfxWarmStartSwingTwistJoint
+
+// pfxSolveHingeJoint = pfxSolveSwingTwistJoint
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_JOINT_HINGE_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint_slider.h

@@ -0,0 +1,57 @@
+/*
+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_JOINT_SLIDER_H
+#define _SCE_PFX_JOINT_SLIDER_H
+
+#include "pfx_joint.h"
+#include "pfx_solver_body.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxSliderJointInitParam {
+	PfxVector3 anchorPoint;
+	PfxVector3 direction;
+	PfxFloat lowerDistance;
+	PfxFloat upperDistance;
+	SCE_PFX_PADDING(1,8)
+	
+	PfxSliderJointInitParam()
+	{
+		anchorPoint = PfxVector3(0.0f);
+		direction = PfxVector3(1.0f,0.0f,0.0f);
+		lowerDistance = 0.0f;
+		upperDistance = 0.0f;
+	}
+};
+
+PfxInt32 pfxInitializeSliderJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	const PfxSliderJointInitParam &param);
+
+// pfxSetupSliderJoint = pfxSetupSwingTwistJoint
+
+// pfxWarmStartSliderJoint = pfxWarmStartSwingTwistJoint
+
+// pfxSolveSliderJoint = pfxSolveSwingTwistJoint
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_JOINT_SLIDER_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint_swing_twist.h

@@ -0,0 +1,70 @@
+/*
+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_JOINT_SWING_TWIST_H
+#define _SCE_PFX_JOINT_SWING_TWIST_H
+
+#include "pfx_joint.h"
+#include "pfx_solver_body.h"
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxSwingTwistJointInitParam {
+	PfxVector3 anchorPoint;
+	PfxVector3 twistAxis;
+	PfxFloat twistLowerAngle;
+	PfxFloat twistUpperAngle;
+	PfxFloat swingLowerAngle;
+	PfxFloat swingUpperAngle;
+	
+	PfxSwingTwistJointInitParam()
+	{
+		anchorPoint = PfxVector3(0.0f);
+		twistAxis = PfxVector3(1.0f,0.0f,0.0f);
+		twistLowerAngle = -0.26f;
+		twistUpperAngle =  0.26f;
+		swingLowerAngle = 0.0f;
+		swingUpperAngle = 0.7f;
+	}
+};
+
+PfxInt32 pfxInitializeSwingTwistJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	const PfxSwingTwistJointInitParam &param);
+
+void pfxSetupSwingTwistJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep);
+
+void pfxWarmStartSwingTwistJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB);
+
+void pfxSolveSwingTwistJoint(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB);
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_JOINT_SWING_TWIST_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_joint_universal.h

@@ -0,0 +1,65 @@
+/*
+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_JOINT_UNIVERSAL_H
+#define _SCE_PFX_JOINT_UNIVERSAL_H
+
+#include "pfx_joint.h"
+#include "pfx_solver_body.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxUniversalJointInitParam {
+	PfxVector3 anchorPoint;
+	PfxVector3 axis;
+	PfxFloat swing1LowerAngle;
+	PfxFloat swing1UpperAngle;
+	PfxFloat swing2LowerAngle;
+	PfxFloat swing2UpperAngle;
+	
+	PfxUniversalJointInitParam()
+	{
+		anchorPoint = PfxVector3(0.0f);
+		axis = PfxVector3(1.0f,0.0f,0.0f);
+		swing1LowerAngle = -0.7f;
+		swing1UpperAngle =  0.7f;
+		swing2LowerAngle = -0.7f;
+		swing2UpperAngle =  0.7f;
+	}
+};
+
+PfxInt32 pfxInitializeUniversalJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	const PfxUniversalJointInitParam &param);
+
+void pfxSetupUniversalJoint(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep);
+
+// pfxWarmStartUniversalJoint = pfxWarmStartSwingTwistJoint
+
+// pfxSolveUniversalJoint = pfxSolveSwingTwistJoint
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_JOINT_UNIVERSAL_H

Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_solver_body.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_SOLVER_BODY_H
+#define _SCE_PFX_SOLVER_BODY_H
+
+#include "../base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct SCE_PFX_ALIGNED(128) PfxSolverBody {
+	PfxVector3 m_deltaLinearVelocity;
+	PfxVector3 m_deltaAngularVelocity;
+	PfxQuat    m_orientation;
+	PfxMatrix3 m_inertiaInv;
+	PfxFloat   m_massInv;
+	PfxUInt32  m_motionType;
+	SCE_PFX_PADDING(1,24)
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_SOLVER_BODY_H

Extras/PhysicsEffects/include/physics_effects/base_level/sort/pfx_sort.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_SORT_H
+#define _SCE_PFX_SORT_H
+
+#include "pfx_sort_data.h"
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Single Sort
+
+void pfxSort(PfxSortData16 *data,PfxSortData16 *buff,unsigned int n);
+void pfxSort(PfxSortData32 *data,PfxSortData32 *buff,unsigned int n);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_SORT_H

Extras/PhysicsEffects/include/physics_effects/base_level/sort/pfx_sort_data.h

@@ -0,0 +1,82 @@
+/*
+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_SORT_DATA_H
+#define _SCE_PFX_SORT_DATA_H
+
+#include "../base/pfx_common.h"
+
+
+
+namespace sce {
+namespace PhysicsEffects {
+
+#define SCE_PFX_SENTINEL_KEY	0xffffffff
+
+struct SCE_PFX_ALIGNED(16) PfxSortData16 {
+	union {
+		PfxUInt8   i8data[16];
+		PfxUInt16  i16data[8];
+		PfxUInt32  i32data[4];
+	};
+
+void set8(int slot,PfxUInt8 data)   {i8data[slot] = data;}
+void set16(int slot,PfxUInt16 data) {i16data[slot] = data;}
+void set32(int slot,PfxUInt32 data) {i32data[slot] = data;}
+PfxUInt8 get8(int slot)   const {return i8data[slot];}
+PfxUInt16 get16(int slot) const {return i16data[slot];}
+PfxUInt32 get32(int slot) const {return i32data[slot];}
+};
+
+struct SCE_PFX_ALIGNED(16) PfxSortData32 {
+	union {
+		PfxUInt8   i8data[32];
+		PfxUInt16  i16data[16];
+		PfxUInt32  i32data[8];
+	};
+
+void set8(int slot,PfxUInt8 data)   {i8data[slot] = data;}
+void set16(int slot,PfxUInt16 data) {i16data[slot] = data;}
+void set32(int slot,PfxUInt32 data) {i32data[slot] = data;}
+PfxUInt8 get8(int slot)   const {return i8data[slot];}
+PfxUInt16 get16(int slot) const {return i16data[slot];}
+PfxUInt32 get32(int slot) const {return i32data[slot];}
+};
+
+SCE_PFX_FORCE_INLINE
+void pfxSetKey(PfxSortData16 &sortData,PfxUInt32 key) {sortData.set32(3,key);}
+
+SCE_PFX_FORCE_INLINE
+PfxUInt32 pfxGetKey(const PfxSortData16 &sortData) {return sortData.get32(3);}
+
+SCE_PFX_FORCE_INLINE
+void pfxSetKey(PfxSortData32 &sortData,PfxUInt32 key) {sortData.set32(7,key);}
+
+SCE_PFX_FORCE_INLINE
+PfxUInt32 pfxGetKey(const PfxSortData32 &sortData) {return sortData.get32(7);}
+
+SCE_PFX_FORCE_INLINE
+PfxUInt32 pfxCreateUniqueKey(PfxUInt32 i,PfxUInt32 j)
+{
+	PfxUInt32 minIdx = SCE_PFX_MIN(i,j);
+	PfxUInt32 maxIdx = SCE_PFX_MAX(i,j);
+	return (maxIdx<<16)|(minIdx&0xffff);
+}
+
+} // namespace PhysicsEffects
+} // namespace sce
+
+#endif // _SCE_PFX_SORT_DATA_H

Extras/PhysicsEffects/include/physics_effects/low_level/broadphase/pfx_broadphase.h

@@ -0,0 +1,124 @@
+/*
+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_BROADPHASE_H_
+#define _SCE_PFX_BROADPHASE_H_
+
+#include "../../base_level/broadphase/pfx_broadphase_pair.h"
+#include "../../base_level/broadphase/pfx_broadphase_proxy.h"
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+#include "../../base_level/collision/pfx_collidable.h"
+#include "../task/pfx_task_manager.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Update Broadphase Proxies
+
+#define SCE_PFX_OUT_OF_WORLD_BEHAVIOR_FIX_MOTION   0x01
+#define SCE_PFX_OUT_OF_WORLD_BEHAVIOR_REMOVE_PROXY 0x02
+
+struct PfxUpdateBroadphaseProxiesParam {
+	void *workBuff;
+	PfxUInt32 workBytes;
+	PfxBroadphaseProxy *proxiesX;
+	PfxBroadphaseProxy *proxiesY;
+	PfxBroadphaseProxy *proxiesZ;
+	PfxBroadphaseProxy *proxiesXb;
+	PfxBroadphaseProxy *proxiesYb;
+	PfxBroadphaseProxy *proxiesZb;
+	PfxRigidState *offsetRigidStates;
+	PfxCollidable *offsetCollidables;
+	PfxUInt32 numRigidBodies;
+	PfxUInt32 outOfWorldBehavior;
+	PfxVector3 worldCenter;
+	PfxVector3 worldExtent;
+	
+	PfxUpdateBroadphaseProxiesParam() : outOfWorldBehavior(0) {}
+};
+
+struct PfxUpdateBroadphaseProxiesResult {
+	PfxInt32 numOutOfWorldProxies;
+};
+
+PfxUInt32 pfxGetWorkBytesOfUpdateBroadphaseProxies(PfxUInt32 numRigidBodies);
+
+PfxUInt32 pfxGetWorkBytesOfUpdateBroadphaseProxies(PfxUInt32 numRigidBodies,PfxUInt32 maxTasks);
+
+PfxInt32 pfxUpdateBroadphaseProxies(PfxUpdateBroadphaseProxiesParam &param,PfxUpdateBroadphaseProxiesResult &result);
+
+PfxInt32 pfxUpdateBroadphaseProxies(PfxUpdateBroadphaseProxiesParam &param,PfxUpdateBroadphaseProxiesResult &result,PfxTaskManager *taskManager);
+
+///////////////////////////////////////////////////////////////////////////////
+// Find Pairs
+
+struct PfxFindPairsParam {
+	void *workBuff;
+	PfxUInt32 workBytes;
+	void *pairBuff;
+	PfxUInt32 pairBytes;
+	PfxBroadphaseProxy *proxies;
+	PfxUInt32 numProxies;
+	PfxUInt32 maxPairs;
+	int axis;
+};
+
+struct PfxFindPairsResult {
+	PfxBroadphasePair *pairs;
+	PfxUInt32 numPairs;
+};
+
+PfxUInt32 pfxGetWorkBytesOfFindPairs(PfxUInt32 maxPairs,PfxUInt32 maxTasks=1);
+PfxUInt32 pfxGetPairBytesOfFindPairs(PfxUInt32 maxPairs);
+
+PfxInt32 pfxFindPairs(PfxFindPairsParam &param,PfxFindPairsResult &result);
+
+PfxInt32 pfxFindPairs(PfxFindPairsParam &param,PfxFindPairsResult &result,PfxTaskManager *taskManager);
+
+///////////////////////////////////////////////////////////////////////////////
+// Decompose Pairs
+
+struct PfxDecomposePairsParam {
+	void *workBuff;
+	PfxUInt32 workBytes;
+	void *pairBuff;
+	PfxUInt32 pairBytes;
+	PfxBroadphasePair *previousPairs;
+	PfxUInt32 numPreviousPairs;
+	PfxBroadphasePair *currentPairs;
+	PfxUInt32 numCurrentPairs;
+};
+
+struct PfxDecomposePairsResult {
+	PfxBroadphasePair *outNewPairs;
+	PfxUInt32 numOutNewPairs;
+	PfxBroadphasePair *outKeepPairs;
+	PfxUInt32 numOutKeepPairs;
+	PfxBroadphasePair *outRemovePairs;
+	PfxUInt32 numOutRemovePairs;
+};
+
+PfxUInt32 pfxGetWorkBytesOfDecomposePairs(PfxUInt32 numPreviousPairs,PfxUInt32 numCurrentPairs,int maxTasks=1);
+PfxUInt32 pfxGetPairBytesOfDecomposePairs(PfxUInt32 numPreviousPairs,PfxUInt32 numCurrentPairs);
+
+PfxInt32 pfxDecomposePairs(PfxDecomposePairsParam &param,PfxDecomposePairsResult &result);
+
+PfxInt32 pfxDecomposePairs(PfxDecomposePairsParam &param,PfxDecomposePairsResult &result,PfxTaskManager *taskManager);
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif /* _SCE_PFX_BROADPHASE_H_ */

Extras/PhysicsEffects/include/physics_effects/low_level/collision/pfx_batched_ray_cast.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_BATCHED_RAY_CAST_H
+#define _SCE_PFX_BATCHED_RAY_CAST_H
+
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+#include "../../base_level/collision/pfx_collidable.h"
+#include "../../base_level/broadphase/pfx_broadphase_proxy.h"
+#include "../task/pfx_task_manager.h"
+#include "pfx_ray_cast.h"
+
+///////////////////////////////////////////////////////////////////////////////
+// Batched RayCast
+namespace sce {
+namespace PhysicsEffects {
+
+void pfxCastRays(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,PfxRayCastParam &param);
+
+void pfxCastRays(PfxRayInput *rayInputs,PfxRayOutput *rayOutputs,int numRays,PfxRayCastParam &param,PfxTaskManager *taskManager);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_BATCHED_RAY_CAST_H

Extras/PhysicsEffects/include/physics_effects/low_level/collision/pfx_collision_detection.h

@@ -0,0 +1,48 @@
+/*
+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_COLLISION_DETECTION_H_
+#define _SCE_PFX_COLLISION_DETECTION_H_
+
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+#include "../../base_level/collision/pfx_collidable.h"
+#include "../../base_level/collision/pfx_contact_manifold.h"
+#include "../../base_level/solver/pfx_constraint_pair.h"
+#include "../task/pfx_task_manager.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Detect Collision
+
+struct PfxDetectCollisionParam {
+	PfxConstraintPair *contactPairs;
+	PfxUInt32 numContactPairs;
+	PfxContactManifold *offsetContactManifolds;
+	PfxRigidState *offsetRigidStates;
+	PfxCollidable *offsetCollidables;
+	PfxUInt32 numRigidBodies;
+};
+
+PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param);
+
+PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param,PfxTaskManager *taskManager);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif /* _SCE_PFX_COLLISION_DETECTION_H_ */

Extras/PhysicsEffects/include/physics_effects/low_level/collision/pfx_island_generation.h

@@ -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
+*/
+
+#ifndef _SCE_PFX_ISLAND_GENERATION_H_
+#define _SCE_PFX_ISLAND_GENERATION_H_
+
+#include "../../base_level/solver/pfx_constraint_pair.h"
+#include "../task/pfx_task_manager.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Island Generation
+
+struct PfxIsland;
+struct PfxIslandUnit;
+
+struct PfxGenerateIslandParam {
+	void *islandBuff;
+	PfxUInt32 islandBytes;
+	PfxConstraintPair *pairs;
+	PfxUInt32 numPairs;
+	PfxUInt32 numObjects;
+};
+
+struct PfxGenerateIslandResult {
+	PfxIsland *island;
+};
+
+//J	アイランド情報を使い終わるまでislandBuffを破棄しないでください
+//E Keep islandBuff while the island is used
+PfxUInt32 pfxGetIslandBytesOfGenerateIsland(PfxUInt32 numObjects);
+
+PfxInt32 pfxGenerateIsland(PfxGenerateIslandParam &param,PfxGenerateIslandResult &result);
+
+//E Access to island information
+//J	アイランド情報へのアクセス
+
+//E Get the number of islands
+//J アイランド数を取得する
+PfxUInt32 pfxGetNumIslands(const PfxIsland *islands);
+
+//E Get a island unit in a island
+//J 指定したアイランドに含まれるユニット（剛体）にアクセス
+PfxIslandUnit *pfxGetFirstUnitInIsland(const PfxIsland *islands,PfxUInt32 islandId);
+PfxIslandUnit *pfxGetNextUnitInIsland(const PfxIslandUnit *islandUnit);
+PfxUInt32 pfxGetUnitId(const PfxIslandUnit *islandUnit);
+
+//E Get an index of an island which includes a specific unit
+//J 指定したユニット（剛体）のアイランドインデックスを取得する
+PfxUInt32 pfxGetIslandId(const PfxIsland *islands,PfxUInt32 unitId);
+
+//E Add pairs and construct islands
+//J アイランドにペアを追加する
+PfxInt32 pfxAppendPairs(PfxIsland *island,PfxConstraintPair *pairs,PfxUInt32 numPairs);
+
+//E Reset islands
+//J	アイランドリセット
+void pfxResetIsland(PfxIsland *island);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif /* _SCE_PFX_ISLAND_GENERATION_H_ */

Extras/PhysicsEffects/include/physics_effects/low_level/collision/pfx_ray_cast.h

@@ -0,0 +1,50 @@
+/*
+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_RAY_CAST_H
+#define _SCE_PFX_RAY_CAST_H
+
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+#include "../../base_level/collision/pfx_collidable.h"
+#include "../../base_level/collision/pfx_ray.h"
+#include "../../base_level/broadphase/pfx_broadphase_proxy.h"
+
+///////////////////////////////////////////////////////////////////////////////
+// RayCast
+
+namespace sce {
+namespace PhysicsEffects {
+
+struct PfxRayCastParam {
+	PfxRigidState *offsetRigidStates;
+	PfxCollidable *offsetCollidables;
+	PfxBroadphaseProxy *proxiesX;
+	PfxBroadphaseProxy *proxiesY;
+	PfxBroadphaseProxy *proxiesZ;
+	PfxBroadphaseProxy *proxiesXb;
+	PfxBroadphaseProxy *proxiesYb;
+	PfxBroadphaseProxy *proxiesZb;
+	PfxUInt32 numProxies;
+	SCE_PFX_PADDING(1,12)
+	PfxVector3 rangeCenter;
+	PfxVector3 rangeExtent;
+};
+
+void pfxCastSingleRay(const PfxRayInput &rayInput,PfxRayOutput &rayOutput,const PfxRayCastParam &param);
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_RAY_CAST_H

Extras/PhysicsEffects/include/physics_effects/low_level/collision/pfx_refresh_contacts.h

@@ -0,0 +1,46 @@
+/*
+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_REFRESH_CONTACTS_H
+#define _SCE_PFX_REFRESH_CONTACTS_H
+
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+#include "../../base_level/solver/pfx_constraint_pair.h"
+#include "../../base_level/collision/pfx_contact_manifold.h"
+#include "../task/pfx_task_manager.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Refresh Contacts
+
+struct PfxRefreshContactsParam {
+	PfxConstraintPair *contactPairs;
+	PfxUInt32 numContactPairs;
+	PfxContactManifold *offsetContactManifolds;
+	PfxRigidState *offsetRigidStates;
+	PfxUInt32 numRigidBodies;
+};
+
+PfxInt32 pfxRefreshContacts(PfxRefreshContactsParam &param);
+
+PfxInt32 pfxRefreshContacts(PfxRefreshContactsParam &param,PfxTaskManager *taskManager);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_REFRESH_CONTACTS_H

Extras/PhysicsEffects/include/physics_effects/low_level/pfx_low_level_include.h

@@ -0,0 +1,46 @@
+/*
+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_LOW_LEVEL_INCLUDE_H
+#define _SCE_PFX_LOW_LEVEL_INCLUDE_H
+
+///////////////////////////////////////////////////////////////////////////////
+// Physics Effects Low Level Headers
+
+// Include base level headers
+#include "../base_level/pfx_base_level_include.h"
+
+// Include low level headers
+#include "broadphase/pfx_broadphase.h"
+
+#include "collision/pfx_collision_detection.h"
+#include "collision/pfx_refresh_contacts.h"
+#include "collision/pfx_batched_ray_cast.h"
+#include "collision/pfx_ray_cast.h"
+#include "collision/pfx_island_generation.h"
+
+#include "solver/pfx_constraint_solver.h"
+#include "solver/pfx_update_rigid_states.h"
+
+#include "sort/pfx_parallel_sort.h"
+
+// ARA begin insert new code 
+#ifdef USE_PTHREADS
+#include "task/pfx_pthreads.h"
+#endif
+// ARA end
+
+#endif // _SCE_PFX_LOW_LEVEL_INCLUDE_H

Extras/PhysicsEffects/include/physics_effects/low_level/solver/pfx_constraint_solver.h

@@ -0,0 +1,123 @@
+/*
+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_SOLVER_H
+#define _SCE_PFX_CONSTRAINT_SOLVER_H
+
+#include "../../base_level/rigidbody/pfx_rigid_body.h"
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+#include "../../base_level/solver/pfx_solver_body.h"
+#include "../../base_level/solver/pfx_constraint_pair.h"
+#include "../../base_level/solver/pfx_joint.h"
+#include "../../base_level/collision/pfx_contact_manifold.h"
+
+#include "../task/pfx_task_manager.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Setup Solver Bodies
+
+struct PfxSetupSolverBodiesParam {
+	PfxRigidState *states;
+	PfxRigidBody *bodies;
+	PfxSolverBody *solverBodies;
+	PfxUInt32 numRigidBodies;
+};
+
+PfxInt32 pfxSetupSolverBodies(PfxSetupSolverBodiesParam &param);
+
+PfxInt32 pfxSetupSolverBodies(PfxSetupSolverBodiesParam &param,PfxTaskManager *taskManager);
+
+///////////////////////////////////////////////////////////////////////////////
+// Setup Constraints
+
+struct PfxSetupContactConstraintsParam {
+	PfxConstraintPair *contactPairs;
+	PfxUInt32 numContactPairs;
+	PfxContactManifold *offsetContactManifolds;
+	PfxRigidState *offsetRigidStates;
+	PfxRigidBody *offsetRigidBodies;
+	PfxSolverBody *offsetSolverBodies;
+	PfxUInt32 numRigidBodies;
+	PfxFloat timeStep;
+	PfxFloat separateBias;
+	
+	PfxSetupContactConstraintsParam()
+	{
+		timeStep = 0.016f;
+		separateBias = 0.2f;
+	}
+};
+
+PfxInt32 pfxSetupContactConstraints(PfxSetupContactConstraintsParam &param);
+
+PfxInt32 pfxSetupContactConstraints(PfxSetupContactConstraintsParam &param,PfxTaskManager *taskManager);
+
+struct PfxSetupJointConstraintsParam {
+	PfxConstraintPair *jointPairs;
+	PfxUInt32 numJointPairs;
+	PfxJoint *offsetJoints;
+	PfxRigidState *offsetRigidStates;
+	PfxRigidBody *offsetRigidBodies;
+	PfxSolverBody *offsetSolverBodies;
+	PfxUInt32 numRigidBodies;
+	PfxFloat timeStep;
+
+	PfxSetupJointConstraintsParam()
+	{
+		timeStep = 0.016f;
+	}
+};
+
+PfxInt32 pfxSetupJointConstraints(PfxSetupJointConstraintsParam &param);
+
+PfxInt32 pfxSetupJointConstraints(PfxSetupJointConstraintsParam &param,PfxTaskManager *taskManager);
+
+///////////////////////////////////////////////////////////////////////////////
+// Solve Constraints
+
+struct PfxSolveConstraintsParam {
+	void *workBuff;
+	PfxUInt32 workBytes;
+	PfxConstraintPair *contactPairs;
+	PfxUInt32 numContactPairs;
+	PfxContactManifold *offsetContactManifolds;
+	PfxConstraintPair *jointPairs;
+	PfxUInt32 numJointPairs;
+	PfxJoint *offsetJoints;
+	PfxRigidState *offsetRigidStates;
+	PfxSolverBody *offsetSolverBodies;
+	PfxUInt32 numRigidBodies;
+	PfxUInt32 iteration;
+	
+	PfxSolveConstraintsParam()
+	{
+		iteration = 5;
+	}
+};
+
+PfxUInt32 pfxGetWorkBytesOfSolveConstraints(PfxUInt32 numRigidBodies,PfxUInt32 numContactPairs,PfxUInt32 numJointPairs,PfxUInt32 maxTasks=1);
+
+PfxInt32 pfxSolveConstraints(PfxSolveConstraintsParam &param);
+
+PfxInt32 pfxSolveConstraints(PfxSolveConstraintsParam &param,PfxTaskManager *taskManager);
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_CONSTRAINT_SOLVER_H
+

Extras/PhysicsEffects/include/physics_effects/low_level/solver/pfx_joint_constraint_func.h

@@ -0,0 +1,69 @@
+/*
+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_JOINT_CONSTRAINT_FUNC_H_
+#define _SCE_PFX_JOINT_CONSTRAINT_FUNC_H_
+
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+#include "../../base_level/solver/pfx_solver_body.h"
+#include "../../base_level/solver/pfx_joint.h"
+#include "../../base_level/solver/pfx_joint_constraint.h"
+
+namespace sce {
+namespace PhysicsEffects {
+///////////////////////////////////////////////////////////////////////////////
+// Setup Joint Constraint Func
+
+typedef void (*PfxSetupJointConstraintFunc)(
+	PfxJoint &joint,
+	const PfxRigidState &stateA,
+	const PfxRigidState &stateB,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB,
+	PfxFloat timeStep);
+
+PfxSetupJointConstraintFunc pfxGetSetupJointConstraintFunc(PfxUInt8 jointType);
+
+int pfxSetSetupJointConstraintFunc(PfxUInt8 jointType,PfxSetupJointConstraintFunc func);
+
+///////////////////////////////////////////////////////////////////////////////
+// Warm Start Joint Constraint Func
+
+typedef void (*PfxWarmStartJointConstraintFunc)(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB);
+
+PfxWarmStartJointConstraintFunc pfxGetWarmStartJointConstraintFunc(PfxUInt8 jointType);
+
+int pfxSetWarmStartJointConstraintFunc(PfxUInt8 jointType,PfxWarmStartJointConstraintFunc func);
+
+///////////////////////////////////////////////////////////////////////////////
+// Solve Joint Constraint Func
+
+typedef void (*PfxSolveJointConstraintFunc)(
+	PfxJoint &joint,
+	PfxSolverBody &solverBodyA,
+	PfxSolverBody &solverBodyB);
+
+PfxSolveJointConstraintFunc pfxGetSolveJointConstraintFunc(PfxUInt8 jointType);
+
+int pfxSetSolveJointConstraintFunc(PfxUInt8 jointType,PfxSolveJointConstraintFunc func);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif /* _PFX_JOINT_CONSTRAINT_FUNC_H_ */

Extras/PhysicsEffects/include/physics_effects/low_level/solver/pfx_update_rigid_states.h

@@ -0,0 +1,45 @@
+/*
+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_UPDATE_RIGID_STATES_H_
+#define _SCE_PFX_UPDATE_RIGID_STATES_H_
+
+#include "../../base_level/rigidbody/pfx_rigid_body.h"
+#include "../../base_level/rigidbody/pfx_rigid_state.h"
+
+#include "../task/pfx_task_manager.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+///////////////////////////////////////////////////////////////////////////////
+// Update Rigid Body States
+
+struct PfxUpdateRigidStatesParam {
+	PfxRigidState *states;
+	PfxRigidBody *bodies;
+	PfxUInt32 numRigidBodies;
+	PfxFloat timeStep;
+};
+
+PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param);
+
+PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param,PfxTaskManager *taskManager);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif /* _SCE_PFX_UPDATE_RIGID_STATES_H_ */

Extras/PhysicsEffects/include/physics_effects/low_level/sort/pfx_parallel_sort.h

@@ -0,0 +1,40 @@
+/*
+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_SORT_H
+#define _SCE_PFX_PARALLEL_SORT_H
+
+#include "../../base_level/sort/pfx_sort_data.h"
+#include "../task/pfx_task_manager.h"
+
+///////////////////////////////////////////////////////////////////////////////
+// Parallel Sort
+namespace sce {
+namespace PhysicsEffects {
+
+PfxInt32 pfxParallelSort(PfxSortData16 *data,PfxUInt32 numData,void *workBuff,PfxUInt32 workBytes);
+
+PfxInt32 pfxParallelSort(PfxSortData32 *data,PfxUInt32 numData,void *workBuff,PfxUInt32 workBytes);
+
+PfxInt32 pfxParallelSort(PfxSortData16 *data,PfxUInt32 numData,void *workBuff,PfxUInt32 workBytes,
+	PfxTaskManager *taskManager);
+
+PfxInt32 pfxParallelSort(PfxSortData32 *data,PfxUInt32 numData,void *workBuff,PfxUInt32 workBytes,
+	PfxTaskManager *taskManager);
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_PARALLEL_SORT_H

Extras/PhysicsEffects/include/physics_effects/low_level/task/pfx_pthreads.h

@@ -0,0 +1,55 @@
+/*
+ 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_PTHREADS_H
+#define _SCE_PFX_PTHREADS_H
+
+#include "../../base_level/base/pfx_common.h"
+#include <errno.h>
+
+namespace sce {
+namespace PhysicsEffects {
+
+class PfxBarrier;
+class PfxCriticalSection;
+class PfxTaskManager;
+
+#define SCE_PFX_CHECK_PTHREADS_OUTCOME(result) \
+    if(0 != result) \
+	{ \
+        SCE_PFX_PRINTF("pthreads problem at line %i in file %s: %i %d\n", __LINE__, __FILE__, result, errno); \
+    }
+
+PfxBarrier *PfxCreateBarrierPthreads(int n);
+PfxCriticalSection *PfxCreateCriticalSectionPthreads();
+PfxTaskManager *PfxCreateTaskManagerPthreads(PfxUInt32 numTasks,PfxUInt32 maxTasks,void *workBuff,PfxUInt32 workBytes);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_PTHREADS_H

Extras/PhysicsEffects/include/physics_effects/low_level/task/pfx_sync_components.h

@@ -0,0 +1,53 @@
+/*
+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_SYNC_COMPONENTS_H
+#define _SCE_PFX_SYNC_COMPONENTS_H
+
+#include "../../base_level/base/pfx_common.h"
+
+//J スレッド間の同期をとるための同期コンポネント
+//E Components for threads sychronization
+namespace sce {
+namespace PhysicsEffects {
+
+class PfxBarrier {
+public:
+	PfxBarrier() {}
+	virtual ~PfxBarrier() {}
+
+	virtual void sync() = 0;
+	virtual void setMaxCount(int n) = 0;
+	virtual int  getMaxCount() = 0;
+};
+
+class PfxCriticalSection {
+public:
+	PfxCriticalSection() {}
+	virtual ~PfxCriticalSection() {}
+
+PfxUInt32 m_commonBuff[32];
+
+	virtual PfxUInt32 getSharedParam(int i) = 0;
+	virtual void setSharedParam(int i,PfxUInt32 p) = 0;
+
+	virtual void lock() = 0;
+	virtual void unlock() = 0;
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_SYNC_COMPONENTS_H

Extras/PhysicsEffects/include/physics_effects/low_level/task/pfx_task_manager.h

@@ -0,0 +1,96 @@
+/*
+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_TASK_MANAGER_H
+#define _SCE_PFX_TASK_MANAGER_H
+
+#include "../../base_level/base/pfx_common.h"
+#include "../../base_level/base/pfx_heap_manager.h"
+#include "pfx_sync_components.h"
+
+#define SCE_PFX_IO_BUFF_BYTES 1048576
+namespace sce {
+namespace PhysicsEffects {
+
+//J 並列処理するためのタスクマネージャクラス
+//E Task manager class for parallel computation
+
+struct PfxTaskArg
+{
+	int taskId;
+	int maxTasks;
+	PfxBarrier *barrier;
+	PfxCriticalSection *criticalSection;
+	void *io;
+	PfxUInt32 data[4];
+};
+
+typedef void (*PfxTaskEntry)(PfxTaskArg *arg);
+
+class PfxTaskManager
+{
+protected:
+	PfxUInt32 m_numTasks;
+	PfxUInt32 m_maxTasks;
+	SCE_PFX_PADDING(1,4)
+	PfxUInt8 SCE_PFX_ALIGNED(16) m_ioBuff[SCE_PFX_IO_BUFF_BYTES];
+	PfxHeapManager m_pool;
+	PfxHeapManager m_ioPool;
+	PfxTaskEntry m_taskEntry;
+	PfxTaskArg *m_taskArg;
+	SCE_PFX_PADDING(2,8)
+
+	PfxTaskManager() : m_pool(NULL,0),m_ioPool(NULL,0) {}
+
+public:
+	void *allocate(size_t bytes) {return m_ioPool.allocate(bytes);}
+	void deallocate(void *p) {m_ioPool.deallocate(p);}
+	void clearPool() {m_ioPool.clear();}
+
+	virtual PfxUInt32 getSharedParam(int i) = 0;
+	virtual void setSharedParam(int i,PfxUInt32 p) = 0;
+
+	virtual void startTask(int taskId,void *io,PfxUInt32 data1,PfxUInt32 data2,PfxUInt32 data3,PfxUInt32 data4) = 0;
+	virtual void waitTask(int &taskId,PfxUInt32 &data1,PfxUInt32 &data2,PfxUInt32 &data3,PfxUInt32 &data4) = 0;
+
+	virtual void setTaskEntry(void *entry) {m_taskEntry = (PfxTaskEntry)entry;}
+
+public:
+	PfxTaskManager(PfxUInt32 numTasks,PfxUInt32 maxTasks,void *workBuff,PfxUInt32 workBytes)
+		: m_pool((unsigned char*)workBuff,workBytes),m_ioPool(m_ioBuff,SCE_PFX_IO_BUFF_BYTES)
+	{
+		SCE_PFX_ASSERT(numTasks>0);
+		SCE_PFX_ASSERT(numTasks<=maxTasks);
+		m_numTasks = numTasks;
+		m_maxTasks = maxTasks;
+		m_taskArg = (PfxTaskArg*)m_pool.allocate(sizeof(PfxTaskArg)*m_maxTasks);
+	}
+
+	virtual ~PfxTaskManager()
+	{
+		m_pool.clear();
+	}
+	
+	virtual PfxUInt32 getNumTasks() const {return m_numTasks;}
+	virtual void setNumTasks(PfxUInt32 tasks) {m_numTasks = SCE_PFX_MIN(tasks,m_maxTasks);}
+	
+	virtual void initialize() = 0;
+	virtual void finalize() = 0;
+};
+
+} //namespace PhysicsEffects
+} //namespace sce
+#endif // _SCE_PFX_TASK_MANAGER_H

Extras/PhysicsEffects/include/physics_effects/util/pfx_mass.h

@@ -0,0 +1,60 @@
+/*
+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
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+//E Mass , Inertia tensor calculation
+//J 質量・慣性テンソルの算出
+
+#ifndef _SCE_PFX_MASS_H
+#define _SCE_PFX_MASS_H
+
+#include "../base_level/base/pfx_common.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+// Box
+PfxFloat pfxCalcMassBox(PfxFloat density,const PfxVector3 &halfExtent);
+PfxMatrix3 pfxCalcInertiaBox(const PfxVector3 &halfExtent,PfxFloat mass);
+
+// Sphere
+PfxFloat pfxCalcMassSphere(PfxFloat density,PfxFloat radius);
+PfxMatrix3 pfxCalcInertiaSphere(PfxFloat radius,PfxFloat mass);
+
+// Cylinder
+PfxFloat pfxCalcMassCylinder(PfxFloat density,PfxFloat halfLength,PfxFloat radius);
+PfxMatrix3 pfxCalcInertiaCylinderX(PfxFloat halfLength,PfxFloat radius,PfxFloat mass);
+PfxMatrix3 pfxCalcInertiaCylinderY(PfxFloat halfLength,PfxFloat radius,PfxFloat mass);
+PfxMatrix3 pfxCalcInertiaCylinderZ(PfxFloat halfLength,PfxFloat radius,PfxFloat mass);
+
+///////////////////////////////////////////////////////////////////////////////
+//E Mass convertion
+//J 質量の移動・回転・合成
+
+// translation
+//E returns translated inertia tensor
+//J 移動後の慣性テンソルを返します
+PfxMatrix3 pfxMassTranslate(PfxFloat mass,const PfxMatrix3 &inertia,const PfxVector3 &translation);
+
+// rotation
+//E returns rotated inertia tensor
+//J 回転後の慣性テンソルを返します
+PfxMatrix3 pfxMassRotate(const PfxMatrix3 &inertia,const PfxMatrix3 &rotate);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_MASS_H

Extras/PhysicsEffects/include/physics_effects/util/pfx_mesh_creator.h

@@ -0,0 +1,96 @@
+/*
+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_CREATOR_H
+#define _SCE_PFX_MESH_CREATOR_H
+
+#include "../base_level/collision/pfx_large_tri_mesh.h"
+
+namespace sce {
+namespace PhysicsEffects {
+
+//J フラグに指定する値
+//E Specify these values to a flag parameter
+#define SCE_PFX_MESH_FLAG_NORMAL_FLIP		0x01
+#define SCE_PFX_MESH_FLAG_16BIT_INDEX		0x02 
+#define SCE_PFX_MESH_FLAG_32BIT_INDEX		0x04
+#define SCE_PFX_MESH_FLAG_AUTO_ELIMINATION	0x08
+#define SCE_PFX_MESH_FLAG_AUTO_THICKNESS	0x10
+
+///////////////////////////////////////////////////////////////////////////////
+// Convex Mesh
+
+struct PfxCreateConvexMeshParam {
+	PfxUInt32 flag;
+	PfxFloat *verts;
+	PfxUInt32 numVerts;
+	void *triangles;
+	PfxUInt32 numTriangles;
+	PfxUInt32 vertexStrideBytes;
+	PfxUInt32 triangleStrideBytes;
+	
+	PfxCreateConvexMeshParam()
+	{
+		flag = SCE_PFX_MESH_FLAG_16BIT_INDEX|SCE_PFX_MESH_FLAG_AUTO_ELIMINATION;
+		verts = NULL;
+		triangles = NULL;
+		numVerts = 0;
+		numTriangles = 0;
+		vertexStrideBytes = sizeof(PfxFloat)*3;
+		triangleStrideBytes = sizeof(PfxUInt16)*3;
+	}
+};
+
+PfxInt32 pfxCreateConvexMesh(PfxConvexMesh &convex,const PfxCreateConvexMeshParam &param);
+
+///////////////////////////////////////////////////////////////////////////////
+// Large Mesh
+
+struct PfxCreateLargeTriMeshParam {
+	PfxUInt32 flag;
+	PfxFloat *verts;
+	PfxUInt32 numVerts;
+	void *triangles;
+	PfxUInt32 numTriangles;
+	PfxUInt32 vertexStrideBytes;
+	PfxUInt32 triangleStrideBytes;
+	PfxUInt32 numFacetsLimit;
+	PfxFloat islandsRatio;
+	PfxFloat defaultThickness;
+	
+	PfxCreateLargeTriMeshParam()
+	{
+		flag = SCE_PFX_MESH_FLAG_16BIT_INDEX|SCE_PFX_MESH_FLAG_AUTO_ELIMINATION;
+		verts = NULL;
+		triangles = NULL;
+		numVerts = 0;
+		numTriangles = 0;
+		vertexStrideBytes = sizeof(PfxFloat)*3;
+		triangleStrideBytes = sizeof(PfxUInt16)*3;
+		numFacetsLimit = 15;
+		islandsRatio = 0.2f;
+		defaultThickness = 0.025f;
+	}
+};
+
+PfxInt32 pfxCreateLargeTriMesh(PfxLargeTriMesh &lmesh,const PfxCreateLargeTriMeshParam &param);
+
+void pfxReleaseLargeTriMesh(PfxLargeTriMesh &lmesh);
+
+} //namespace PhysicsEffects
+} //namespace sce
+
+#endif // _SCE_PFX_MESH_CREATOR_H

Extras/PhysicsEffects/include/physics_effects/util/pfx_util_include.h

@@ -0,0 +1,26 @@
+/*
+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_INCLUDE_H
+#define _SCE_PFX_UTIL_INCLUDE_H
+
+///////////////////////////////////////////////////////////////////////////////
+// Physics Effects Utility Headers
+
+#include "pfx_mass.h"
+#include "pfx_mesh_creator.h"
+
+#endif // _SCE_PFX_UTIL_INCLUDE_H

Extras/PhysicsEffects/include/vecmath/neon/boolInVec.h

@@ -0,0 +1,238 @@
+/*
+   Copyright (C) 2006-2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _BOOLINVEC_SCALAR_H
+#define _BOOLINVEC_SCALAR_H
+
+#include <math.h>
+namespace Vectormath {
+
+class floatInVec;
+
+//--------------------------------------------------------------------------------------------------
+// boolInVec class
+//
+
+class boolInVec
+{
+private:
+    unsigned int mData;
+
+public:
+    // Default constructor; does no initialization
+    //
+    inline boolInVec( ) { };
+
+    // Construct from a value converted from float
+    //
+    inline boolInVec(floatInVec vec);
+
+    // Explicit cast from bool
+    //
+    explicit inline boolInVec(bool scalar);
+
+    // Explicit cast to bool
+    //
+    inline bool getAsBool() const;
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+    // Implicit cast to bool
+    //
+    inline operator bool() const;
+#endif
+
+    // Boolean negation operator
+    //
+    inline const boolInVec operator ! () const;
+
+    // Assignment operator
+    //
+    inline boolInVec& operator = (boolInVec vec);
+
+    // Boolean and assignment operator
+    //
+    inline boolInVec& operator &= (boolInVec vec);
+
+    // Boolean exclusive or assignment operator
+    //
+    inline boolInVec& operator ^= (boolInVec vec);
+
+    // Boolean or assignment operator
+    //
+    inline boolInVec& operator |= (boolInVec vec);
+
+};
+
+// Equal operator
+//
+inline const boolInVec operator == (boolInVec vec0, boolInVec vec1);
+
+// Not equal operator
+//
+inline const boolInVec operator != (boolInVec vec0, boolInVec vec1);
+
+// And operator
+//
+inline const boolInVec operator & (boolInVec vec0, boolInVec vec1);
+
+// Exclusive or operator
+//
+inline const boolInVec operator ^ (boolInVec vec0, boolInVec vec1);
+
+// Or operator
+//
+inline const boolInVec operator | (boolInVec vec0, boolInVec vec1);
+
+// Conditionally select between two values
+//
+inline const boolInVec select(boolInVec vec0, boolInVec vec1, boolInVec select_vec1);
+
+
+} // namespace Vectormath
+
+
+//--------------------------------------------------------------------------------------------------
+// boolInVec implementation
+//
+
+#include "floatInVec.h"
+
+namespace Vectormath {
+
+inline
+boolInVec::boolInVec(floatInVec vec)
+{
+    *this = (vec != floatInVec(0.0f));
+}
+
+inline
+boolInVec::boolInVec(bool scalar)
+{
+    mData = -(int)scalar;
+}
+
+inline
+bool
+boolInVec::getAsBool() const
+{
+    return (mData > 0);
+}
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+inline
+boolInVec::operator bool() const
+{
+    return getAsBool();
+}
+#endif
+
+inline
+const boolInVec
+boolInVec::operator ! () const
+{
+    return boolInVec(!mData);
+}
+
+inline
+boolInVec&
+boolInVec::operator = (boolInVec vec)
+{
+    mData = vec.mData;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator &= (boolInVec vec)
+{
+    *this = *this & vec;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator ^= (boolInVec vec)
+{
+    *this = *this ^ vec;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator |= (boolInVec vec)
+{
+    *this = *this | vec;
+    return *this;
+}
+
+inline
+const boolInVec
+operator == (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() == vec1.getAsBool());
+}
+
+inline
+const boolInVec
+operator != (boolInVec vec0, boolInVec vec1)
+{
+    return !(vec0 == vec1);
+}
+
+inline
+const boolInVec
+operator & (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() & vec1.getAsBool());
+}
+
+inline
+const boolInVec
+operator | (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() | vec1.getAsBool());
+}
+
+inline
+const boolInVec
+operator ^ (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() ^ vec1.getAsBool());
+}
+
+inline
+const boolInVec
+select(boolInVec vec0, boolInVec vec1, boolInVec select_vec1)
+{
+    return (select_vec1.getAsBool() == 0) ? vec0 : vec1;
+}
+
+} // namespace Vectormath
+
+#endif // boolInVec_h

Extras/PhysicsEffects/include/vecmath/neon/floatInVec.h

@@ -0,0 +1,357 @@
+/*
+   Copyright (C) 2006-2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _FLOATINVEC__SCALAR_H
+#define _FLOATINVEC__SCALAR_H
+
+#include <math.h>
+namespace Vectormath {
+
+class boolInVec;
+
+//--------------------------------------------------------------------------------------------------
+// floatInVec class
+//
+
+// A class representing a scalar float value contained in a vector register
+// This class does not support fastmath
+class floatInVec
+{
+private:
+    float mData;
+
+public:
+    // Default constructor; does no initialization
+    //
+    inline floatInVec( ) { };
+
+    // Construct from a value converted from bool
+    //
+    inline floatInVec(boolInVec vec);
+
+    // Explicit cast from float
+    //
+    explicit inline floatInVec(float scalar);
+
+    // Explicit cast to float
+    //
+    inline float getAsFloat() const;
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+    // Implicit cast to float
+    //
+    inline operator float() const;
+#endif
+
+    // Post increment (add 1.0f)
+    //
+    inline const floatInVec operator ++ (int);
+
+    // Post decrement (subtract 1.0f)
+    //
+    inline const floatInVec operator -- (int);
+
+    // Pre increment (add 1.0f)
+    //
+    inline floatInVec& operator ++ ();
+
+    // Pre decrement (subtract 1.0f)
+    //
+    inline floatInVec& operator -- ();
+
+    // Negation operator
+    //
+    inline const floatInVec operator - () const;
+
+    // Assignment operator
+    //
+    inline floatInVec& operator = (floatInVec vec);
+
+    // Multiplication assignment operator
+    //
+    inline floatInVec& operator *= (floatInVec vec);
+
+    // Division assignment operator
+    //
+    inline floatInVec& operator /= (floatInVec vec);
+
+    // Addition assignment operator
+    //
+    inline floatInVec& operator += (floatInVec vec);
+
+    // Subtraction assignment operator
+    //
+    inline floatInVec& operator -= (floatInVec vec);
+
+};
+
+// Multiplication operator
+//
+inline const floatInVec operator * (floatInVec vec0, floatInVec vec1);
+
+// Division operator
+//
+inline const floatInVec operator / (floatInVec vec0, floatInVec vec1);
+
+// Addition operator
+//
+inline const floatInVec operator + (floatInVec vec0, floatInVec vec1);
+
+// Subtraction operator
+//
+inline const floatInVec operator - (floatInVec vec0, floatInVec vec1);
+
+// Less than operator
+//
+inline const boolInVec operator < (floatInVec vec0, floatInVec vec1);
+
+// Less than or equal operator
+//
+inline const boolInVec operator <= (floatInVec vec0, floatInVec vec1);
+
+// Greater than operator
+//
+inline const boolInVec operator > (floatInVec vec0, floatInVec vec1);
+
+// Greater than or equal operator
+//
+inline const boolInVec operator >= (floatInVec vec0, floatInVec vec1);
+
+// Equal operator
+//
+inline const boolInVec operator == (floatInVec vec0, floatInVec vec1);
+
+// Not equal operator
+//
+inline const boolInVec operator != (floatInVec vec0, floatInVec vec1);
+
+// Conditionally select between two values
+//
+inline const floatInVec select(floatInVec vec0, floatInVec vec1, boolInVec select_vec1);
+
+
+} // namespace Vectormath
+
+
+//--------------------------------------------------------------------------------------------------
+// floatInVec implementation
+//
+
+#include "boolInVec.h"
+
+namespace Vectormath {
+
+inline
+floatInVec::floatInVec(boolInVec vec)
+{
+    mData = float(vec.getAsBool());
+}
+
+inline
+floatInVec::floatInVec(float scalar)
+{
+    mData = scalar;
+}
+
+inline
+float
+floatInVec::getAsFloat() const
+{
+    return mData;
+}
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+inline
+floatInVec::operator float() const
+{
+    return getAsFloat();
+}
+#endif
+
+inline
+const floatInVec
+floatInVec::operator ++ (int)
+{
+    float olddata = mData;
+    operator ++();
+    return floatInVec(olddata);
+}
+
+inline
+const floatInVec
+floatInVec::operator -- (int)
+{
+    float olddata = mData;
+    operator --();
+    return floatInVec(olddata);
+}
+
+inline
+floatInVec&
+floatInVec::operator ++ ()
+{
+    *this += floatInVec(1.0f);
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator -- ()
+{
+    *this -= floatInVec(1.0f);
+    return *this;
+}
+
+inline
+const floatInVec
+floatInVec::operator - () const
+{
+    return floatInVec(-mData);
+}
+
+inline
+floatInVec&
+floatInVec::operator = (floatInVec vec)
+{
+    mData = vec.mData;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator *= (floatInVec vec)
+{
+    *this = *this * vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator /= (floatInVec vec)
+{
+    *this = *this / vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator += (floatInVec vec)
+{
+    *this = *this + vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator -= (floatInVec vec)
+{
+    *this = *this - vec;
+    return *this;
+}
+
+inline
+const floatInVec
+operator * (floatInVec vec0, floatInVec vec1)
+{
+    return floatInVec(vec0.getAsFloat() * vec1.getAsFloat());
+}
+
+inline
+const floatInVec
+operator / (floatInVec num, floatInVec den)
+{
+    return floatInVec(num.getAsFloat() / den.getAsFloat());
+}
+
+inline
+const floatInVec
+operator + (floatInVec vec0, floatInVec vec1)
+{
+    return floatInVec(vec0.getAsFloat() + vec1.getAsFloat());
+}
+
+inline
+const floatInVec
+operator - (floatInVec vec0, floatInVec vec1)
+{
+    return floatInVec(vec0.getAsFloat() - vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator < (floatInVec vec0, floatInVec vec1)
+{
+    return boolInVec(vec0.getAsFloat() < vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator <= (floatInVec vec0, floatInVec vec1)
+{
+    return !(vec0 > vec1);
+}
+
+inline
+const boolInVec
+operator > (floatInVec vec0, floatInVec vec1)
+{
+    return boolInVec(vec0.getAsFloat() > vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator >= (floatInVec vec0, floatInVec vec1)
+{
+    return !(vec0 < vec1);
+}
+
+inline
+const boolInVec
+operator == (floatInVec vec0, floatInVec vec1)
+{
+    return boolInVec(vec0.getAsFloat() == vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator != (floatInVec vec0, floatInVec vec1)
+{
+    return !(vec0 == vec1);
+}
+
+inline
+const floatInVec
+select(floatInVec vec0, floatInVec vec1, boolInVec select_vec1)
+{
+    return (select_vec1.getAsBool() == 0) ? vec0 : vec1;
+}
+
+} // namespace Vectormath
+
+#endif // floatInVec_h

Extras/PhysicsEffects/include/vecmath/neon/quat_aos.h

@@ -0,0 +1,446 @@
+/*
+   Copyright (C) 2006-2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _VECTORMATH_QUAT_AOS_CPP_H
+#define _VECTORMATH_QUAT_AOS_CPP_H
+
+//-----------------------------------------------------------------------------
+// Definitions
+
+#ifndef _VECTORMATH_INTERNAL_FUNCTIONS
+#define _VECTORMATH_INTERNAL_FUNCTIONS
+
+#endif
+
+namespace Vectormath {
+namespace Aos {
+
+inline Quat::Quat( const Quat & quat )
+{
+    mX = quat.mX;
+    mY = quat.mY;
+    mZ = quat.mZ;
+    mW = quat.mW;
+}
+
+inline Quat::Quat( float _x, float _y, float _z, float _w )
+{
+    mX = _x;
+    mY = _y;
+    mZ = _z;
+    mW = _w;
+}
+
+inline Quat::Quat( const Vector3 & xyz, float _w )
+{
+    this->setXYZ( xyz );
+    this->setW( _w );
+}
+
+inline Quat::Quat( const Vector4 & vec )
+{
+    mX = vec.getX();
+    mY = vec.getY();
+    mZ = vec.getZ();
+    mW = vec.getW();
+}
+
+inline Quat::Quat( float scalar )
+{
+    mX = scalar;
+    mY = scalar;
+    mZ = scalar;
+    mW = scalar;
+}
+
+inline const Quat Quat::identity( )
+{
+    return Quat( 0.0f, 0.0f, 0.0f, 1.0f );
+}
+
+inline const Quat lerp( float t, const Quat & quat0, const Quat & quat1 )
+{
+    return ( quat0 + ( ( quat1 - quat0 ) * t ) );
+}
+
+inline const Quat slerp( float t, const Quat & unitQuat0, const Quat & unitQuat1 )
+{
+    Quat start;
+    float recipSinAngle, scale0, scale1, cosAngle, angle;
+    cosAngle = dot( unitQuat0, unitQuat1 );
+    if ( cosAngle < 0.0f ) {
+        cosAngle = -cosAngle;
+        start = ( -unitQuat0 );
+    } else {
+        start = unitQuat0;
+    }
+    if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
+        angle = acosf( cosAngle );
+        recipSinAngle = ( 1.0f / sinf( angle ) );
+        scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
+        scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
+    } else {
+        scale0 = ( 1.0f - t );
+        scale1 = t;
+    }
+    return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
+}
+
+inline const Quat squad( float t, const Quat & unitQuat0, const Quat & unitQuat1, const Quat & unitQuat2, const Quat & unitQuat3 )
+{
+    Quat tmp0, tmp1;
+    tmp0 = slerp( t, unitQuat0, unitQuat3 );
+    tmp1 = slerp( t, unitQuat1, unitQuat2 );
+    return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
+}
+
+inline void loadXYZW( Quat & quat, const float * fptr )
+{
+    quat = Quat( fptr[0], fptr[1], fptr[2], fptr[3] );
+}
+
+inline void storeXYZW( const Quat & quat, float * fptr )
+{
+    fptr[0] = quat.getX();
+    fptr[1] = quat.getY();
+    fptr[2] = quat.getZ();
+    fptr[3] = quat.getW();
+}
+
+inline Quat & Quat::operator =( const Quat & quat )
+{
+    mX = quat.mX;
+    mY = quat.mY;
+    mZ = quat.mZ;
+    mW = quat.mW;
+    return *this;
+}
+
+inline Quat & Quat::setXYZ( const Vector3 & vec )
+{
+    mX = vec.getX();
+    mY = vec.getY();
+    mZ = vec.getZ();
+    return *this;
+}
+
+inline const Vector3 Quat::getXYZ( ) const
+{
+    return Vector3( mX, mY, mZ );
+}
+
+inline Quat & Quat::setX( float _x )
+{
+    mX = _x;
+    return *this;
+}
+
+inline float Quat::getX( ) const
+{
+    return mX;
+}
+
+inline Quat & Quat::setY( float _y )
+{
+    mY = _y;
+    return *this;
+}
+
+inline float Quat::getY( ) const
+{
+    return mY;
+}
+
+inline Quat & Quat::setZ( float _z )
+{
+    mZ = _z;
+    return *this;
+}
+
+inline float Quat::getZ( ) const
+{
+    return mZ;
+}
+
+inline Quat & Quat::setW( float _w )
+{
+    mW = _w;
+    return *this;
+}
+
+inline float Quat::getW( ) const
+{
+    return mW;
+}
+
+inline Quat & Quat::setElem( int idx, float value )
+{
+    *(&mX + idx) = value;
+    return *this;
+}
+
+inline float Quat::getElem( int idx ) const
+{
+    return *(&mX + idx);
+}
+
+inline float & Quat::operator []( int idx )
+{
+    return *(&mX + idx);
+}
+
+inline float Quat::operator []( int idx ) const
+{
+    return *(&mX + idx);
+}
+
+inline const Quat Quat::operator +( const Quat & quat ) const
+{
+    return Quat(
+        ( mX + quat.mX ),
+        ( mY + quat.mY ),
+        ( mZ + quat.mZ ),
+        ( mW + quat.mW )
+    );
+}
+
+inline const Quat Quat::operator -( const Quat & quat ) const
+{
+    return Quat(
+        ( mX - quat.mX ),
+        ( mY - quat.mY ),
+        ( mZ - quat.mZ ),
+        ( mW - quat.mW )
+    );
+}
+
+inline const Quat Quat::operator *( float scalar ) const
+{
+    return Quat(
+        ( mX * scalar ),
+        ( mY * scalar ),
+        ( mZ * scalar ),
+        ( mW * scalar )
+    );
+}
+
+inline Quat & Quat::operator +=( const Quat & quat )
+{
+    *this = *this + quat;
+    return *this;
+}
+
+inline Quat & Quat::operator -=( const Quat & quat )
+{
+    *this = *this - quat;
+    return *this;
+}
+
+inline Quat & Quat::operator *=( float scalar )
+{
+    *this = *this * scalar;
+    return *this;
+}
+
+inline const Quat Quat::operator /( float scalar ) const
+{
+    return Quat(
+        ( mX / scalar ),
+        ( mY / scalar ),
+        ( mZ / scalar ),
+        ( mW / scalar )
+    );
+}
+
+inline Quat & Quat::operator /=( float scalar )
+{
+    *this = *this / scalar;
+    return *this;
+}
+
+inline const Quat Quat::operator -( ) const
+{
+    return Quat(
+        -mX,
+        -mY,
+        -mZ,
+        -mW
+    );
+}
+
+inline const Quat operator *( float scalar, const Quat & quat )
+{
+    return quat * scalar;
+}
+
+inline float dot( const Quat & quat0, const Quat & quat1 )
+{
+    float result;
+    result = ( quat0.getX() * quat1.getX() );
+    result = ( result + ( quat0.getY() * quat1.getY() ) );
+    result = ( result + ( quat0.getZ() * quat1.getZ() ) );
+    result = ( result + ( quat0.getW() * quat1.getW() ) );
+    return result;
+}
+
+inline float norm( const Quat & quat )
+{
+    float result;
+    result = ( quat.getX() * quat.getX() );
+    result = ( result + ( quat.getY() * quat.getY() ) );
+    result = ( result + ( quat.getZ() * quat.getZ() ) );
+    result = ( result + ( quat.getW() * quat.getW() ) );
+    return result;
+}
+
+inline float length( const Quat & quat )
+{
+    return ::sqrtf( norm( quat ) );
+}
+
+inline const Quat normalize( const Quat & quat )
+{
+    float lenSqr, lenInv;
+    lenSqr = norm( quat );
+    lenInv = ( 1.0f / sqrtf( lenSqr ) );
+    return Quat(
+        ( quat.getX() * lenInv ),
+        ( quat.getY() * lenInv ),
+        ( quat.getZ() * lenInv ),
+        ( quat.getW() * lenInv )
+    );
+}
+
+inline const Quat Quat::rotation( const Vector3 & unitVec0, const Vector3 & unitVec1 )
+{
+    float cosHalfAngleX2, recipCosHalfAngleX2;
+    cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f + dot( unitVec0, unitVec1 ) ) ) );
+    recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
+    return Quat( ( cross( unitVec0, unitVec1 ) * recipCosHalfAngleX2 ), ( cosHalfAngleX2 * 0.5f ) );
+}
+
+inline const Quat Quat::rotation( float radians, const Vector3 & unitVec )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( ( unitVec * s ), c );
+}
+
+inline const Quat Quat::rotationX( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( s, 0.0f, 0.0f, c );
+}
+
+inline const Quat Quat::rotationY( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( 0.0f, s, 0.0f, c );
+}
+
+inline const Quat Quat::rotationZ( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( 0.0f, 0.0f, s, c );
+}
+
+inline const Quat Quat::operator *( const Quat & quat ) const
+{
+    return Quat(
+        ( ( ( ( mW * quat.mX ) + ( mX * quat.mW ) ) + ( mY * quat.mZ ) ) - ( mZ * quat.mY ) ),
+        ( ( ( ( mW * quat.mY ) + ( mY * quat.mW ) ) + ( mZ * quat.mX ) ) - ( mX * quat.mZ ) ),
+        ( ( ( ( mW * quat.mZ ) + ( mZ * quat.mW ) ) + ( mX * quat.mY ) ) - ( mY * quat.mX ) ),
+        ( ( ( ( mW * quat.mW ) - ( mX * quat.mX ) ) - ( mY * quat.mY ) ) - ( mZ * quat.mZ ) )
+    );
+}
+
+inline Quat & Quat::operator *=( const Quat & quat )
+{
+    *this = *this * quat;
+    return *this;
+}
+
+inline const Vector3 rotate( const Quat & quat, const Vector3 & vec )
+{
+    float tmpX, tmpY, tmpZ, tmpW;
+    tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
+    tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
+    tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
+    tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
+    return Vector3(
+        ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
+        ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
+        ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
+    );
+}
+
+inline const Quat conj( const Quat & quat )
+{
+    return Quat( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
+}
+
+inline const Quat select( const Quat & quat0, const Quat & quat1, bool select1 )
+{
+    return Quat(
+        ( select1 )? quat1.getX() : quat0.getX(),
+        ( select1 )? quat1.getY() : quat0.getY(),
+        ( select1 )? quat1.getZ() : quat0.getZ(),
+        ( select1 )? quat1.getW() : quat0.getW()
+    );
+}
+
+#ifdef _VECTORMATH_DEBUG
+
+inline void print( const Quat & quat )
+{
+    printf( "( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
+}
+
+inline void print( const Quat & quat, const char * name )
+{
+    printf( "%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
+}
+
+#endif
+
+} // namespace Aos
+} // namespace Vectormath
+
+#endif

Extras/PhysicsEffects/include/vecmath/neon/vectormath_neon_assembly_implementations.S

@@ -0,0 +1,310 @@
+@
+@ 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.
+@
+@----------------------------------------------------------------
+@
+@ This file contains ARM/NEON assembly versions of some vectormath
+@ atomic functions. We have implemented here instead of inline assembly
+@ because we have found gcc 4.4.3 to be too inconsistent and inadequate
+@ to properly support either NEON intrinsics or inline assembly. (See
+@ the inline assembly version of the vector3 dot product, which is
+@ contained in vec_aos.h but compiled out, for an example of a simple
+@ inline assembly function that wreaks havok if used.)
+@
+@ Note that the certain NEON registers must be preserved across
+@ function calls according to the following document:
+@
+@ "Procedure Call Standard for the ARM? Architecture," ARM document
+@ number ARM IHI 0042D, current through ABI release 2.08,
+@ 16th October, 2009, section 5.1.2.1
+@
+@ The registers are: q4-q7 (and their double-word and single word
+@ counterparts)
+@
+@ These functions preserve all non-scratch general purpose registers
+@ as well as the ones listed, and so we do not need to have extra
+@ code to do the register preservation
+@
+	.syntax unified
+	.arch armv7-a
+	.fpu neon
+	.thumb
+	.text
+	.align 2
+
+@----------------------------------------------------------------
+@ pfxVector3DotProductNEON
+@
+@ Vector3 dot product, result stored directly to memory
+@
+@ Result stored in memory pointed to by r2. r2 must point to
+@ memory sufficient to store two 32 bit floats, though only the
+@ first value is of interest
+@----------------------------------------------------------------
+	.global	pfxVector3DotProductNEON
+	.thumb_func
+pfxVector3DotProductNEON:
+	.fnstart
+	vld1.32 {d0,d1}, [r1]		@ input <x2,y2,z2,?> = d0,d1\n\t"
+	vld1.32 {d2,d3}, [r0]		@ input <x1,y1,z1,?> = d2,d3\n\t"
+	vmul.f32 d4, d0, d2			@ d4 = <x1*x2,y1*y2>\n\t"
+	vpadd.f32 d4, d4, d4		@ d4 = <x1*x2 + y1*y2, x1*x2 + y1*y2>\n\t"
+	vmla.f32 s8, s2, s6			@ s8 = <x1*x2 + y1*y2 + z1*z2\n\t"
+	vst1.32 {d4}, [r2]			@ save result to memory. supports double/quad word only. We only care about first word\n\t"
+	bx	lr
+	.fnend
+
+@----------------------------------------------------------------
+@ pfxVector4DotProductNEON
+@
+@ Vector4 dot product, result stored directly to memory
+@
+@ Result stored in memory pointed to by r2. r2 must point to
+@ memory sufficient to store two 32 bit floats, though only the
+@ first value is of interest
+@----------------------------------------------------------------
+	.global	pfxVector4DotProductNEON
+	.thumb_func
+pfxVector4DotProductNEON:
+	.fnstart
+	vld1.32	{d16,d17}, [r0]		@ input <x1,y1,z1,w1>
+	vld1.32	{d18,d19}, [r1]		@ input <x2,y2,z2,w2>
+	vmul.f32 d14, d16, d18		@ d14=<x1*x2,y1*y2>
+	@ non-fused multiple accumulate
+	vmla.f32 d14, d17, d19		@ d14=d14+<z1*z2,w1*w2>=<x1*x2+z1*z2,y1*y2+w1*w2>
+	@ fused multiple accumulate - listed here for reference but we use vmla above
+	@ instead since the fused version is not recognized by GNU assembler (as part
+	@ of the gcc 4.4.3 Android distribution)
+	@	vfma.f32 {d14}, d17, d19	@ d14=d14+<z1*z2,w1*w2>=<x1*x2+z1*z2,y1*y2+w1*w2>
+	vpadd.f32 d14, d14, d14		@ add the two halves of d14 together, same result in each lane
+	vst1.32	{d14}, [r2]
+	bx	lr
+	.fnend
+
+@----------------------------------------------------------------
+@ pfxVector3CrossProductNEON
+@
+@ Vector3 cross product, result stored directly to memory
+@
+@ Result stored in memory pointed to by r2. r2 must point to
+@ memory sufficient to store four 32 bit floats, though only the
+@ first 3 values are of interest
+@----------------------------------------------------------------
+	.global	pfxVector3CrossProductNEON
+	.thumb_func
+pfxVector3CrossProductNEON:
+	.fnstart
+	vld1.32	{d18,d19}, [r1]	@ input <x2,y2,z2,w2> = d18,d19
+	vld1.32	{d16,d17}, [r0]	@ input <x1,y1,z1,w1> = d16,d17
+	@ rearrange inputs into convenient order
+	vtrn.32 d18,d19			@  q9 = <x2,z2,y2,w2> = d18,d19
+	vrev64.32 d16,d16		@  q8 = <y1,x1,z1,w1> = d16,d17
+	vrev64.32 d18,d18		@  q9 = <z2,x2,y2,w2> = d18,d19
+	vtrn.32 d16,d17			@  q8 = <y1,z1,x1,w1> = d16,d17
+	@ perform first half of cross product using rearranged inputs
+	vmul.f32 q10, q8, q9	@ q10 = <y1*z2,z1*x2,x1*y2,w1*w2>
+	@ rearrange inputs again
+	vtrn.32 d18,d19			@  q9 = <z2,y2,x2,w2> = d18,d19
+	vrev64.32 d16,d16		@  q8 = <z1,y1,x1,w1> = d16,d17
+	vrev64.32 d18,d18		@  q9 = <y2,z2,x2,w2> = d18,d19
+	vtrn.32 d16,d17			@  q8 = <z1,x1,y1,w1> = d16,d17
+	@ perform last half of cross product using rearranged inputs
+	vmls.f32 q10, q8, q9	@ q10 = <y1*z2-y2*z1,z1*x2-z2*x1,x1*y2-x2*y1,w1*w2-w2*w1>
+	@ and store the result
+	vst1.32	{q10}, [r2]
+	bx	lr
+	.fnend
+
+@----------------------------------------------------------------
+@ pfxMatrix3Matrix3ProductNEON
+@
+@ Matrix3 * Matrix3 product, result stored directly to memory
+@
+@ Result stored in memory pointed to by r2. r2 must point to
+@ memory sufficient to store 12 32-bit floats. The reason for
+@ 12 rather than 9 is that each column vector actually has
+@ 4 32-bit floats rather than just 3....since NEON works with
+@ double and quad vectors.
+@
+@ Note that, since the inputs are loaded into registers then
+@ never used again, r2 can point to one of the inputs, e.g.,
+@ result can be stored back out to one of the input memory
+@ locations.
+@----------------------------------------------------------------
+	.global	pfxMatrix3Matrix3ProductNEON
+	.thumb_func
+pfxMatrix3Matrix3ProductNEON:
+	.fnstart
+	vld1.32 {d16-d19}, [r0]!	@ load first eight elements of matrix 0
+	vld1.32 {d20-d21}, [r0]		@ load second eight elements of matrix 0
+	vld1.32 {d0-d3}, [r1]!		@ load first eight elements of matrix 1
+	vld1.32 {d4-d5}, [r1]		@ load second eight elements of matrix 1
+	vmul.f32 q12, q8, d0[0]		@ rslt col0  = (mat0 col0) * (mat1 col0 elt0)
+	vmul.f32 q13, q8, d2[0]		@ rslt col1  = (mat0 col0) * (mat1 col1 elt0)
+	vmul.f32 q14, q8, d4[0]		@ rslt col2  = (mat0 col0) * (mat1 col2 elt0)
+	vmla.f32 q12, q9, d0[1]		@ rslt col0 += (mat0 col1) * (mat1 col0 elt1)
+	vmla.f32 q13, q9, d2[1]		@ rslt col1 += (mat0 col1) * (mat1 col1 elt1)
+	vmla.f32 q14, q9, d4[1]		@ rslt col2 += (mat0 col1) * (mat1 col2 elt1)
+	vmla.f32 q12, q10, d1[0]	@ rslt col0 += (mat0 col2) * (mat1 col0 elt2)
+	vmla.f32 q13, q10, d3[0]	@ rslt col1 += (mat0 col2) * (mat1 col1 elt2)
+	vmla.f32 q14, q10, d5[0]	@ rslt col2 += (mat0 col2) * (mat1 col2 elt2)
+	vst1.32 {d24-d27}, [r2]!	@ store first eight elements of result (each column has an extra float as stored)
+	vst1.32 {d28-d29}, [r2]		@ store last four elements of result (each column has an extra float as stored)
+	bx	lr
+	.fnend
+
+@----------------------------------------------------------------
+@ pfxMatrix4Matrix4ProductNEON
+@
+@ Matrix4 * Matrix4 product, result stored directly to memory
+@
+@ Result stored in memory pointed to by r2. r2 must point to
+@ memory sufficient to store 16 32 bit floats.
+@
+@ Note that, since the inputs are loaded into registers then
+@ never used again, r2 can point to one of the inputs, e.g.,
+@ result can be stored back out to one of the input memory
+@ locations.
+@----------------------------------------------------------------
+	.global	pfxMatrix4Matrix4ProductNEON
+	.thumb_func
+pfxMatrix4Matrix4ProductNEON:
+	.fnstart
+	vld1.32 {d16-d19}, [r0]!	@ load first eight elements of matrix 0
+	vld1.32 {d20-d23}, [r0]		@ load second eight elements of matrix 0
+	vld1.32 {d0-d3}, [r1]!		@ load first eight elements of matrix 1
+	vld1.32 {d4-d7}, [r1]		@ load second eight elements of matrix 1
+	vmul.f32 q12, q8, d0[0]		@ rslt col0  = (mat0 col0) * (mat1 col0 elt0)
+	vmul.f32 q13, q8, d2[0]		@ rslt col1  = (mat0 col0) * (mat1 col1 elt0)
+	vmul.f32 q14, q8, d4[0]		@ rslt col2  = (mat0 col0) * (mat1 col2 elt0)
+	vmul.f32 q15, q8, d6[0]		@ rslt col3  = (mat0 col0) * (mat1 col3 elt0)
+	vmla.f32 q12, q9, d0[1]		@ rslt col0 += (mat0 col1) * (mat1 col0 elt1)
+	vmla.f32 q13, q9, d2[1]		@ rslt col1 += (mat0 col1) * (mat1 col1 elt1)
+	vmla.f32 q14, q9, d4[1]		@ rslt col2 += (mat0 col1) * (mat1 col2 elt1)
+	vmla.f32 q15, q9, d6[1]		@ rslt col3 += (mat0 col1) * (mat1 col3 elt1)
+	vmla.f32 q12, q10, d1[0]	@ rslt col0 += (mat0 col2) * (mat1 col0 elt2)
+	vmla.f32 q13, q10, d3[0]	@ rslt col1 += (mat0 col2) * (mat1 col1 elt2)
+	vmla.f32 q14, q10, d5[0]	@ rslt col2 += (mat0 col2) * (mat1 col2 elt2)
+	vmla.f32 q15, q10, d7[0]	@ rslt col3 += (mat0 col2) * (mat1 col2 elt2)
+	vmla.f32 q12, q11, d1[1]	@ rslt col0 += (mat0 col3) * (mat1 col0 elt3)
+	vmla.f32 q13, q11, d3[1]	@ rslt col1 += (mat0 col3) * (mat1 col1 elt3)
+	vmla.f32 q14, q11, d5[1]	@ rslt col2 += (mat0 col3) * (mat1 col2 elt3)
+	vmla.f32 q15, q11, d7[1]	@ rslt col3 += (mat0 col3) * (mat1 col3 elt3)
+	vst1.32 {d24-d27}, [r2]!	@ store first eight elements of result
+	vst1.32 {d28-d31}, [r2]		@ store second eight elements of result
+	bx	lr
+	.fnend
+
+@----------------------------------------------------------------
+@ pfxTransform3OrthoInverseNEON
+@
+@ Computes the ortho inverse of a Transform 3.
+@
+@ Result stored in memory pointed to by r1. r1 must point to
+@ memory sufficient to store 16 32 bit floats.
+@
+@ Note that, since the inputs are loaded into registers then
+@ never used again, r1 can point to one of the inputs, e.g.,
+@ result can be stored back out to one of the input memory
+@ locations.
+@
+@ Note that this expects the inputs to have 4 floats per row,
+@ (to be consistent with NEON quad vector), and the last float
+@ in each row should be 0.0 for the math to work out.
+@----------------------------------------------------------------
+	.global	pfxTransform3OrthoInverseNEON
+	.thumb_func
+pfxTransform3OrthoInverseNEON:
+	.fnstart
+	@ direct load the first column of the ortho inverse result
+	vld1.f32 d0[0], [r0]!
+	vld1.f32 d2[0], [r0]!
+	vld1.f32 d4[0], [r0]!
+	vld1.f32 d1[1], [r0]!
+
+	@ direct load the second column of the ortho inverse result
+	vld1.f32 d0[1], [r0]!
+	vld1.f32 d2[1], [r0]!
+	vld1.f32 d4[1], [r0]!
+	vld1.f32 d3[1], [r0]!
+
+	@ direct load the third column of the ortho inverse result
+	vld1.f32 d1[0], [r0]!
+	vld1.f32 d3[0], [r0]!
+
+	vst1.f32 {d0-d3}, [r1]!		@ store first eight elements of result (1st two columns)
+
+	vld1.f32 d5[0], [r0]!
+	vld1.f32 d5[1], [r0]!
+
+	vst1.f32 {d4-d5}, [r1]!		@ store next four elements of result (3rd column)
+
+	@ move q0 into q8 so we can reuse q0 to load fourth column
+	@ of input. We do this to avoid using q4-q7 (which have to
+	@ be preserved during the function call)....needed to work
+	@ around some limitation rules that prevent us from accessing
+	@ single s registers associated with q8 and above.
+	vmov.f32 q8, q0
+
+	@ direct load the last column of the input
+	vld1.f32 {q0}, [r0]
+
+	@ prepare the last column of the output
+	vmul.f32 q3, q8, d0[0]		@ multiply result column 1 by x coord of input column 3
+	vmla.f32 q3, q1, d0[1]		@ multiply result column 2 by y coord of input column 3 and add
+	vmla.f32 q3, q2, d1[0]		@ multiply result column 3 by z coord of input column 3 and add
+	vneg.f32 q3, q3				@ negate final column
+
+	vst1.f32 {q3}, [r1]			@ store last four elements of result (4th column)
+
+	bx	lr
+	.fnend
+
+@----------------------------------------------------------------
+@ pfxTransform3Vector3MultiplyNEON
+@
+@ Computes the product of a Transform3 and a Vector3, e.g., it
+@ applies the transform to the vector.
+@
+@ Result stored in memory pointed to by r2. r2 must point to
+@ memory sufficient to store 4 32-bit floats.
+@----------------------------------------------------------------
+	.global	pfxTransform3Vector3MultiplyNEON
+	.thumb_func
+pfxTransform3Vector3MultiplyNEON:
+	.fnstart
+
+	vld1.32 {d16-d19}, [r0]!	@ load first eight elements of transform matrix
+	vld1.32 {d20-d21}, [r0]		@ load second eight elements of transform matrix
+	vld1.32 {d0-d1}, [r1]		@ load the four elements of vector3 (last one is just padding)
+	vmul.f32 q12, q8, d0[0]		@ rslt col0  = (mat0 col0) * (mat1 col0 elt0)
+	vmla.f32 q12, q9, d0[1]		@ rslt col0 += (mat0 col1) * (mat1 col0 elt1)
+	vmla.f32 q12, q10, d1[0]	@ rslt col0 += (mat0 col2) * (mat1 col0 elt2)
+	vst1.32 {d24-d25}, [r2]		@ store four elements of result (last one is padding)
+
+	bx	lr
+	.fnend

Extras/PhysicsEffects/include/vecmath/neon/vectormath_neon_assembly_prototypes.h

@@ -0,0 +1,57 @@
+/*
+ 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 _VECTORMATH_NEON_ASSEMBLY_PROTOTYPES_H
+#define _VECTORMATH_NEON_ASSEMBLY_PROTOTYPES_H
+
+// Prototypes for NEON assembly implementations
+extern "C"
+{
+	// NEON assembly implementations of Vector3 functions
+	void pfxVector3DotProductNEON(const Vectormath::Aos::Vector3 &vec0, const Vectormath::Aos::Vector3 &vec1, float *result);
+	void pfxVector3CrossProductNEON(const Vectormath::Aos::Vector3 &vec0, const Vectormath::Aos::Vector3 &vec1,
+									Vectormath::Aos::Vector3 &result);
+
+	// NEON assembly implementations of Matrix3 functions
+	void pfxMatrix3Matrix3ProductNEON(const Vectormath::Aos::Matrix3 &mat0, const Vectormath::Aos::Matrix3 &mat1,
+										Vectormath::Aos::Matrix3 &result);
+
+	// NEON assembly implementations of Transform3 functions
+	void pfxTransform3OrthoInverseNEON(const Vectormath::Aos::Transform3 &trn, Vectormath::Aos::Transform3 &result);
+	void pfxTransform3Vector3MultiplyNEON(const Vectormath::Aos::Transform3 &trn, const Vectormath::Aos::Vector3 &vec,
+											Vectormath::Aos::Vector3 &result);
+
+	// NEON assembly implementations of Vector4 functions
+	void pfxVector4DotProductNEON(const Vectormath::Aos::Vector4 &vec0, const Vectormath::Aos::Vector4 &vec1, float *result);
+
+	// NEON assembly implementations of Matrix4 functions
+	void pfxMatrix4Matrix4ProductNEON(const Vectormath::Aos::Matrix4 &mat0, const Vectormath::Aos::Matrix4 &mat1,
+										Vectormath::Aos::Matrix4 &result);
+}
+
+#endif // _VECTORMATH_NEON_ASSEMBLY_PROTOTYPES_H

Extras/PhysicsEffects/include/vecmath/sse/boolInVec.h

@@ -0,0 +1,247 @@
+/*
+   Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _BOOLINVEC_H
+#define _BOOLINVEC_H
+
+#include <math.h>
+
+namespace Vectormath {
+
+class floatInVec;
+
+//--------------------------------------------------------------------------------------------------
+// boolInVec class
+//
+
+class boolInVec
+{
+    private:
+        __m128 mData;
+
+        inline boolInVec(__m128 vec);
+    public:
+        inline boolInVec() {}
+
+        // matches standard type conversions
+        //
+        inline boolInVec(const floatInVec &vec);
+
+        // explicit cast from bool
+        //
+        explicit inline boolInVec(bool scalar);
+
+#ifdef _VECTORMATH_NO_SCALAR_CAST
+        // explicit cast to bool
+        // 
+        inline bool getAsBool() const;
+#else
+        // implicit cast to bool
+        // 
+        inline operator bool() const;
+#endif
+        
+        // get vector data
+        // bool value is splatted across all word slots of vector as 0 (false) or -1 (true)
+        //
+        inline __m128 get128() const;
+
+        // operators
+        //
+        inline const boolInVec operator ! () const;
+        inline boolInVec& operator = (const boolInVec &vec);
+        inline boolInVec& operator &= (const boolInVec &vec);
+        inline boolInVec& operator ^= (const boolInVec &vec);
+        inline boolInVec& operator |= (const boolInVec &vec);
+
+        // friend functions
+        //
+        friend inline const boolInVec operator == (const boolInVec &vec0, const boolInVec &vec1);
+        friend inline const boolInVec operator != (const boolInVec &vec0, const boolInVec &vec1);
+        friend inline const boolInVec operator < (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const boolInVec operator <= (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const boolInVec operator > (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const boolInVec operator >= (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const boolInVec operator == (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const boolInVec operator != (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const boolInVec operator & (const boolInVec &vec0, const boolInVec &vec1);
+        friend inline const boolInVec operator ^ (const boolInVec &vec0, const boolInVec &vec1);
+        friend inline const boolInVec operator | (const boolInVec &vec0, const boolInVec &vec1);
+        friend inline const boolInVec select(const boolInVec &vec0, const boolInVec &vec1, const boolInVec &select_vec1);
+};
+
+//--------------------------------------------------------------------------------------------------
+// boolInVec functions
+//
+
+// operators
+//
+inline const boolInVec operator == (const boolInVec &vec0, const boolInVec &vec1);
+inline const boolInVec operator != (const boolInVec &vec0, const boolInVec &vec1);
+inline const boolInVec operator & (const boolInVec &vec0, const boolInVec &vec1);
+inline const boolInVec operator ^ (const boolInVec &vec0, const boolInVec &vec1);
+inline const boolInVec operator | (const boolInVec &vec0, const boolInVec &vec1);
+
+// select between vec0 and vec1 using boolInVec.
+// false selects vec0, true selects vec1
+//
+inline const boolInVec select(const boolInVec &vec0, const boolInVec &vec1, const boolInVec &select_vec1);
+
+} // namespace Vectormath
+
+//--------------------------------------------------------------------------------------------------
+// boolInVec implementation
+//
+
+#include "floatInVec.h"
+
+namespace Vectormath {
+
+inline
+boolInVec::boolInVec(__m128 vec)
+{
+    mData = vec;
+}
+
+inline
+boolInVec::boolInVec(const floatInVec &vec)
+{
+    *this = (vec != floatInVec(0.0f));
+}
+
+inline
+boolInVec::boolInVec(bool scalar)
+{
+    unsigned int mask = -(int)scalar;
+	mData = _mm_set1_ps(*(float *)&mask); // TODO: Union
+}
+
+#ifdef _VECTORMATH_NO_SCALAR_CAST
+inline
+bool
+boolInVec::getAsBool() const
+#else
+inline
+boolInVec::operator bool() const
+#endif
+{
+	return *(bool *)&mData;
+}
+
+inline
+__m128
+boolInVec::get128() const
+{
+    return mData;
+}
+
+inline
+const boolInVec
+boolInVec::operator ! () const
+{
+    return boolInVec(_mm_andnot_ps(mData, _mm_cmpneq_ps(_mm_setzero_ps(),_mm_setzero_ps())));
+}
+
+inline
+boolInVec&
+boolInVec::operator = (const boolInVec &vec)
+{
+    mData = vec.mData;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator &= (const boolInVec &vec)
+{
+    *this = *this & vec;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator ^= (const boolInVec &vec)
+{
+    *this = *this ^ vec;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator |= (const boolInVec &vec)
+{
+    *this = *this | vec;
+    return *this;
+}
+
+inline
+const boolInVec
+operator == (const boolInVec &vec0, const boolInVec &vec1)
+{
+	return boolInVec(_mm_cmpeq_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+operator != (const boolInVec &vec0, const boolInVec &vec1)
+{
+	return boolInVec(_mm_cmpneq_ps(vec0.get128(), vec1.get128()));
+}
+    
+inline
+const boolInVec
+operator & (const boolInVec &vec0, const boolInVec &vec1)
+{
+	return boolInVec(_mm_and_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+operator | (const boolInVec &vec0, const boolInVec &vec1)
+{
+	return boolInVec(_mm_or_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+operator ^ (const boolInVec &vec0, const boolInVec &vec1)
+{
+	return boolInVec(_mm_xor_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+select(const boolInVec &vec0, const boolInVec &vec1, const boolInVec &select_vec1)
+{
+	return boolInVec(vec_sel(vec0.get128(), vec1.get128(), select_vec1.get128()));
+}
+ 
+} // namespace Vectormath
+
+#endif // boolInVec_h

Extras/PhysicsEffects/include/vecmath/sse/floatInVec.h

@@ -0,0 +1,340 @@
+/*
+   Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _FLOATINVEC_H
+#define _FLOATINVEC_H
+
+#include <math.h>
+#include <xmmintrin.h>
+
+namespace Vectormath {
+
+class boolInVec;
+
+//--------------------------------------------------------------------------------------------------
+// floatInVec class
+//
+
+class floatInVec
+{
+    private:
+        __m128 mData;
+
+    public:
+        inline floatInVec(__m128 vec);
+
+        inline floatInVec() {}
+
+        // matches standard type conversions
+        //
+        inline floatInVec(const boolInVec &vec);
+
+        // construct from a slot of __m128
+        //
+        inline floatInVec(__m128 vec, int slot);
+        
+        // explicit cast from float
+        //
+        explicit inline floatInVec(float scalar);
+
+#ifdef _VECTORMATH_NO_SCALAR_CAST
+        // explicit cast to float
+        // 
+        inline float getAsFloat() const;
+#else
+        // implicit cast to float
+        //
+        inline operator float() const;
+#endif
+
+        // get vector data
+        // float value is splatted across all word slots of vector
+        //
+        inline __m128 get128() const;
+
+        // operators
+        // 
+        inline const floatInVec operator ++ (int);
+        inline const floatInVec operator -- (int);
+        inline floatInVec& operator ++ ();
+        inline floatInVec& operator -- ();
+        inline const floatInVec operator - () const;
+        inline floatInVec& operator = (const floatInVec &vec);
+        inline floatInVec& operator *= (const floatInVec &vec);
+        inline floatInVec& operator /= (const floatInVec &vec);
+        inline floatInVec& operator += (const floatInVec &vec);
+        inline floatInVec& operator -= (const floatInVec &vec);
+
+        // friend functions
+        //
+        friend inline const floatInVec operator * (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const floatInVec operator / (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const floatInVec operator + (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const floatInVec operator - (const floatInVec &vec0, const floatInVec &vec1);
+        friend inline const floatInVec select(const floatInVec &vec0, const floatInVec &vec1, boolInVec select_vec1);
+};
+
+//--------------------------------------------------------------------------------------------------
+// floatInVec functions
+//
+
+// operators
+// 
+inline const floatInVec operator * (const floatInVec &vec0, const floatInVec &vec1);
+inline const floatInVec operator / (const floatInVec &vec0, const floatInVec &vec1);
+inline const floatInVec operator + (const floatInVec &vec0, const floatInVec &vec1);
+inline const floatInVec operator - (const floatInVec &vec0, const floatInVec &vec1);
+inline const boolInVec operator < (const floatInVec &vec0, const floatInVec &vec1);
+inline const boolInVec operator <= (const floatInVec &vec0, const floatInVec &vec1);
+inline const boolInVec operator > (const floatInVec &vec0, const floatInVec &vec1);
+inline const boolInVec operator >= (const floatInVec &vec0, const floatInVec &vec1);
+inline const boolInVec operator == (const floatInVec &vec0, const floatInVec &vec1);
+inline const boolInVec operator != (const floatInVec &vec0, const floatInVec &vec1);
+
+// select between vec0 and vec1 using boolInVec.
+// false selects vec0, true selects vec1
+//
+inline const floatInVec select(const floatInVec &vec0, const floatInVec &vec1, const boolInVec &select_vec1);
+
+} // namespace Vectormath
+
+//--------------------------------------------------------------------------------------------------
+// floatInVec implementation
+//
+
+#include "boolInVec.h"
+
+namespace Vectormath {
+
+inline
+floatInVec::floatInVec(__m128 vec)
+{
+    mData = vec;
+}
+
+inline
+floatInVec::floatInVec(const boolInVec &vec)
+{
+	mData = vec_sel(_mm_setzero_ps(), _mm_set1_ps(1.0f), vec.get128());
+}
+
+inline
+floatInVec::floatInVec(__m128 vec, int slot)
+{
+	SSEFloat v;
+	v.m128 = vec;
+	mData = _mm_set1_ps(v.f[slot]);
+}
+
+inline
+floatInVec::floatInVec(float scalar)
+{
+	mData = _mm_set1_ps(scalar);
+}
+
+#ifdef _VECTORMATH_NO_SCALAR_CAST
+inline
+float
+floatInVec::getAsFloat() const
+#else
+inline
+floatInVec::operator float() const
+#endif
+{
+    return *((float *)&mData);
+}
+
+inline
+__m128
+floatInVec::get128() const
+{
+    return mData;
+}
+
+inline
+const floatInVec
+floatInVec::operator ++ (int)
+{
+    __m128 olddata = mData;
+    operator ++();
+    return floatInVec(olddata);
+}
+
+inline
+const floatInVec
+floatInVec::operator -- (int)
+{
+    __m128 olddata = mData;
+    operator --();
+    return floatInVec(olddata);
+}
+
+inline
+floatInVec&
+floatInVec::operator ++ ()
+{
+    *this += floatInVec(_mm_set1_ps(1.0f));
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator -- ()
+{
+    *this -= floatInVec(_mm_set1_ps(1.0f));
+    return *this;
+}
+
+inline
+const floatInVec
+floatInVec::operator - () const
+{
+    return floatInVec(_mm_sub_ps(_mm_setzero_ps(), mData));
+}
+
+inline
+floatInVec&
+floatInVec::operator = (const floatInVec &vec)
+{
+    mData = vec.mData;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator *= (const floatInVec &vec)
+{
+    *this = *this * vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator /= (const floatInVec &vec)
+{
+    *this = *this / vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator += (const floatInVec &vec)
+{
+    *this = *this + vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator -= (const floatInVec &vec)
+{
+    *this = *this - vec;
+    return *this;
+}
+
+inline
+const floatInVec
+operator * (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return floatInVec(_mm_mul_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const floatInVec
+operator / (const floatInVec &num, const floatInVec &den)
+{
+    return floatInVec(_mm_div_ps(num.get128(), den.get128()));
+}
+
+inline
+const floatInVec
+operator + (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return floatInVec(_mm_add_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const floatInVec
+operator - (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return floatInVec(_mm_sub_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+operator < (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return boolInVec(_mm_cmpgt_ps(vec1.get128(), vec0.get128()));
+}
+
+inline
+const boolInVec
+operator <= (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return boolInVec(_mm_cmpge_ps(vec1.get128(), vec0.get128()));
+}
+
+inline
+const boolInVec
+operator > (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return boolInVec(_mm_cmpgt_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+operator >= (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return boolInVec(_mm_cmpge_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+operator == (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return boolInVec(_mm_cmpeq_ps(vec0.get128(), vec1.get128()));
+}
+
+inline
+const boolInVec
+operator != (const floatInVec &vec0, const floatInVec &vec1)
+{
+    return boolInVec(_mm_cmpneq_ps(vec0.get128(), vec1.get128()));
+}
+    
+inline
+const floatInVec
+select(const floatInVec &vec0, const floatInVec &vec1, const boolInVec &select_vec1)
+{
+    return floatInVec(vec_sel(vec0.get128(), vec1.get128(), select_vec1.get128()));
+}
+
+} // namespace Vectormath
+
+#endif // floatInVec_h

Extras/PhysicsEffects/include/vecmath/sse/quat_aos.h

@@ -0,0 +1,579 @@
+/*
+   Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _VECTORMATH_QUAT_AOS_CPP_H
+#define _VECTORMATH_QUAT_AOS_CPP_H
+
+//-----------------------------------------------------------------------------
+// Definitions
+
+#ifndef _VECTORMATH_INTERNAL_FUNCTIONS
+#define _VECTORMATH_INTERNAL_FUNCTIONS
+
+#endif
+
+namespace Vectormath {
+namespace Aos {
+
+VECTORMATH_FORCE_INLINE void Quat::set128(vec_float4 vec)
+{
+    mVec128 = vec;
+}
+
+VECTORMATH_FORCE_INLINE Quat::Quat( const floatInVec &_x, const floatInVec &_y, const floatInVec &_z, const floatInVec &_w )
+{
+	mVec128 = _mm_unpacklo_ps(
+		_mm_unpacklo_ps( _x.get128(), _z.get128() ),
+		_mm_unpacklo_ps( _y.get128(), _w.get128() ) );
+}
+
+VECTORMATH_FORCE_INLINE Quat::Quat( const Vector3 &xyz, float _w )
+{
+    mVec128 = xyz.get128();
+    _vmathVfSetElement(mVec128, _w, 3);
+}
+
+
+
+VECTORMATH_FORCE_INLINE  Quat::Quat(const Quat& quat)
+{
+	mVec128 = quat.get128();
+}
+
+VECTORMATH_FORCE_INLINE Quat::Quat( float _x, float _y, float _z, float _w )
+{
+	mVec128 = _mm_setr_ps(_x, _y, _z, _w);
+}
+
+
+
+
+
+VECTORMATH_FORCE_INLINE Quat::Quat( const Vector3 &xyz, const floatInVec &_w )
+{
+    mVec128 = xyz.get128();
+    mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
+}
+
+VECTORMATH_FORCE_INLINE Quat::Quat( const Vector4 &vec )
+{
+    mVec128 = vec.get128();
+}
+
+VECTORMATH_FORCE_INLINE Quat::Quat( float scalar )
+{
+    mVec128 = floatInVec(scalar).get128();
+}
+
+VECTORMATH_FORCE_INLINE Quat::Quat( const floatInVec &scalar )
+{
+    mVec128 = scalar.get128();
+}
+
+VECTORMATH_FORCE_INLINE Quat::Quat( __m128 vf4 )
+{
+    mVec128 = vf4;
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::identity( )
+{
+    return Quat( _VECTORMATH_UNIT_0001 );
+}
+
+VECTORMATH_FORCE_INLINE const Quat lerp( float t, const Quat &quat0, const Quat &quat1 )
+{
+    return lerp( floatInVec(t), quat0, quat1 );
+}
+
+VECTORMATH_FORCE_INLINE const Quat lerp( const floatInVec &t, const Quat &quat0, const Quat &quat1 )
+{
+    return ( quat0 + ( ( quat1 - quat0 ) * t ) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat slerp( float t, const Quat &unitQuat0, const Quat &unitQuat1 )
+{
+    return slerp( floatInVec(t), unitQuat0, unitQuat1 );
+}
+
+VECTORMATH_FORCE_INLINE const Quat slerp( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1 )
+{
+    Quat start;
+    vec_float4 scales, scale0, scale1, cosAngle, angle, tttt, oneMinusT, angles, sines;
+    __m128 selectMask;
+    cosAngle = _vmathVfDot4( unitQuat0.get128(), unitQuat1.get128() );
+    selectMask = (__m128)vec_cmpgt( _mm_setzero_ps(), cosAngle );
+    cosAngle = vec_sel( cosAngle, negatef4( cosAngle ), selectMask );
+    start = Quat( vec_sel( unitQuat0.get128(), negatef4( unitQuat0.get128() ), selectMask ) );
+    selectMask = (__m128)vec_cmpgt( _mm_set1_ps(_VECTORMATH_SLERP_TOL), cosAngle );
+    angle = acosf4( cosAngle );
+    tttt = t.get128();
+    oneMinusT = vec_sub( _mm_set1_ps(1.0f), tttt );
+    angles = vec_mergeh( _mm_set1_ps(1.0f), tttt );
+    angles = vec_mergeh( angles, oneMinusT );
+    angles = vec_madd( angles, angle, _mm_setzero_ps() );
+    sines = sinf4( angles );
+    scales = _mm_div_ps( sines, vec_splat( sines, 0 ) );
+    scale0 = vec_sel( oneMinusT, vec_splat( scales, 1 ), selectMask );
+    scale1 = vec_sel( tttt, vec_splat( scales, 2 ), selectMask );
+    return Quat( vec_madd( start.get128(), scale0, vec_mul( unitQuat1.get128(), scale1 ) ) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat squad( float t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )
+{
+    return squad( floatInVec(t), unitQuat0, unitQuat1, unitQuat2, unitQuat3 );
+}
+
+VECTORMATH_FORCE_INLINE const Quat squad( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )
+{
+    return slerp( ( ( floatInVec(2.0f) * t ) * ( floatInVec(1.0f) - t ) ), slerp( t, unitQuat0, unitQuat3 ), slerp( t, unitQuat1, unitQuat2 ) );
+}
+
+VECTORMATH_FORCE_INLINE __m128 Quat::get128( ) const
+{
+    return mVec128;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator =( const Quat &quat )
+{
+    mVec128 = quat.mVec128;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setXYZ( const Vector3 &vec )
+{
+	VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
+	mVec128 = vec_sel( vec.get128(), mVec128, sw );
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const Vector3 Quat::getXYZ( ) const
+{
+    return Vector3( mVec128 );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setX( float _x )
+{
+    _vmathVfSetElement(mVec128, _x, 0);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setX( const floatInVec &_x )
+{
+    mVec128 = _vmathVfInsert(mVec128, _x.get128(), 0);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec Quat::getX( ) const
+{
+    return floatInVec( mVec128, 0 );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setY( float _y )
+{
+    _vmathVfSetElement(mVec128, _y, 1);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setY( const floatInVec &_y )
+{
+    mVec128 = _vmathVfInsert(mVec128, _y.get128(), 1);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec Quat::getY( ) const
+{
+    return floatInVec( mVec128, 1 );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setZ( float _z )
+{
+    _vmathVfSetElement(mVec128, _z, 2);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setZ( const floatInVec &_z )
+{
+    mVec128 = _vmathVfInsert(mVec128, _z.get128(), 2);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec Quat::getZ( ) const
+{
+    return floatInVec( mVec128, 2 );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setW( float _w )
+{
+    _vmathVfSetElement(mVec128, _w, 3);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setW( const floatInVec &_w )
+{
+    mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec Quat::getW( ) const
+{
+    return floatInVec( mVec128, 3 );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setElem( int idx, float value )
+{
+    _vmathVfSetElement(mVec128, value, idx);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::setElem( int idx, const floatInVec &value )
+{
+    mVec128 = _vmathVfInsert(mVec128, value.get128(), idx);
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec Quat::getElem( int idx ) const
+{
+    return floatInVec( mVec128, idx );
+}
+
+VECTORMATH_FORCE_INLINE VecIdx Quat::operator []( int idx )
+{
+    return VecIdx( mVec128, idx );
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec Quat::operator []( int idx ) const
+{
+    return floatInVec( mVec128, idx );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator +( const Quat &quat ) const
+{
+    return Quat( _mm_add_ps( mVec128, quat.mVec128 ) );
+}
+
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator -( const Quat &quat ) const
+{
+    return Quat( _mm_sub_ps( mVec128, quat.mVec128 ) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator *( float scalar ) const
+{
+    return *this * floatInVec(scalar);
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator *( const floatInVec &scalar ) const
+{
+    return Quat( _mm_mul_ps( mVec128, scalar.get128() ) );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator +=( const Quat &quat )
+{
+    *this = *this + quat;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator -=( const Quat &quat )
+{
+    *this = *this - quat;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( float scalar )
+{
+    *this = *this * scalar;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( const floatInVec &scalar )
+{
+    *this = *this * scalar;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator /( float scalar ) const
+{
+    return *this / floatInVec(scalar);
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator /( const floatInVec &scalar ) const
+{
+    return Quat( _mm_div_ps( mVec128, scalar.get128() ) );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator /=( float scalar )
+{
+    *this = *this / scalar;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator /=( const floatInVec &scalar )
+{
+    *this = *this / scalar;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator -( ) const
+{
+	return Quat(_mm_sub_ps( _mm_setzero_ps(), mVec128 ) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat operator *( float scalar, const Quat &quat )
+{
+    return floatInVec(scalar) * quat;
+}
+
+VECTORMATH_FORCE_INLINE const Quat operator *( const floatInVec &scalar, const Quat &quat )
+{
+    return quat * scalar;
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec dot( const Quat &quat0, const Quat &quat1 )
+{
+    return floatInVec( _vmathVfDot4( quat0.get128(), quat1.get128() ), 0 );
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec norm( const Quat &quat )
+{
+    return floatInVec(  _vmathVfDot4( quat.get128(), quat.get128() ), 0 );
+}
+
+VECTORMATH_FORCE_INLINE const floatInVec length( const Quat &quat )
+{
+    return floatInVec(  _mm_sqrt_ps(_vmathVfDot4( quat.get128(), quat.get128() )), 0 );
+}
+
+VECTORMATH_FORCE_INLINE const Quat normalize( const Quat &quat )
+{
+	vec_float4 dot =_vmathVfDot4( quat.get128(), quat.get128());
+    return Quat( _mm_mul_ps( quat.get128(), newtonrapson_rsqrt4( dot ) ) );
+}
+
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotation( const Vector3 &unitVec0, const Vector3 &unitVec1 )
+{
+    Vector3 crossVec;
+    __m128 cosAngle, cosAngleX2Plus2, recipCosHalfAngleX2, cosHalfAngleX2, res;
+    cosAngle = _vmathVfDot3( unitVec0.get128(), unitVec1.get128() );
+    cosAngleX2Plus2 = vec_madd( cosAngle, _mm_set1_ps(2.0f), _mm_set1_ps(2.0f) );
+    recipCosHalfAngleX2 = _mm_rsqrt_ps( cosAngleX2Plus2 );
+    cosHalfAngleX2 = vec_mul( recipCosHalfAngleX2, cosAngleX2Plus2 );
+    crossVec = cross( unitVec0, unitVec1 );
+    res = vec_mul( crossVec.get128(), recipCosHalfAngleX2 );
+	VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
+    res = vec_sel( res, vec_mul( cosHalfAngleX2, _mm_set1_ps(0.5f) ), sw );
+    return Quat( res );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotation( float radians, const Vector3 &unitVec )
+{
+    return rotation( floatInVec(radians), unitVec );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotation( const floatInVec &radians, const Vector3 &unitVec )
+{
+    __m128 s, c, angle, res;
+    angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
+    sincosf4( angle, &s, &c );
+	VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
+    res = vec_sel( vec_mul( unitVec.get128(), s ), c, sw );
+    return Quat( res );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotationX( float radians )
+{
+    return rotationX( floatInVec(radians) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotationX( const floatInVec &radians )
+{
+    __m128 s, c, angle, res;
+    angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
+    sincosf4( angle, &s, &c );
+	VM_ATTRIBUTE_ALIGN16 unsigned int xsw[4] = {0xffffffff, 0, 0, 0};
+	VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
+    res = vec_sel( _mm_setzero_ps(), s, xsw );
+    res = vec_sel( res, c, wsw );
+    return Quat( res );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotationY( float radians )
+{
+    return rotationY( floatInVec(radians) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotationY( const floatInVec &radians )
+{
+    __m128 s, c, angle, res;
+    angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
+    sincosf4( angle, &s, &c );
+	VM_ATTRIBUTE_ALIGN16 unsigned int ysw[4] = {0, 0xffffffff, 0, 0};
+	VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
+    res = vec_sel( _mm_setzero_ps(), s, ysw );
+    res = vec_sel( res, c, wsw );
+    return Quat( res );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotationZ( float radians )
+{
+    return rotationZ( floatInVec(radians) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::rotationZ( const floatInVec &radians )
+{
+    __m128 s, c, angle, res;
+    angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
+    sincosf4( angle, &s, &c );
+	VM_ATTRIBUTE_ALIGN16 unsigned int zsw[4] = {0, 0, 0xffffffff, 0};
+	VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
+    res = vec_sel( _mm_setzero_ps(), s, zsw );
+    res = vec_sel( res, c, wsw );
+    return Quat( res );
+}
+
+VECTORMATH_FORCE_INLINE const Quat Quat::operator *( const Quat &quat ) const
+{
+    __m128 ldata, rdata, qv, tmp0, tmp1, tmp2, tmp3;
+    __m128 product, l_wxyz, r_wxyz, xy, qw;
+    ldata = mVec128;
+    rdata = quat.mVec128;
+    tmp0 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,0,2,1) );
+    tmp1 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,1,0,2) );
+    tmp2 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,1,0,2) );
+    tmp3 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,0,2,1) );
+    qv = vec_mul( vec_splat( ldata, 3 ), rdata );
+    qv = vec_madd( vec_splat( rdata, 3 ), ldata, qv );
+    qv = vec_madd( tmp0, tmp1, qv );
+    qv = vec_nmsub( tmp2, tmp3, qv );
+    product = vec_mul( ldata, rdata );
+    l_wxyz = vec_sld( ldata, ldata, 12 );
+    r_wxyz = vec_sld( rdata, rdata, 12 );
+    qw = vec_nmsub( l_wxyz, r_wxyz, product );
+    xy = vec_madd( l_wxyz, r_wxyz, product );
+    qw = vec_sub( qw, vec_sld( xy, xy, 8 ) );
+	VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
+    return Quat( vec_sel( qv, qw, sw ) );
+}
+
+VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( const Quat &quat )
+{
+    *this = *this * quat;
+    return *this;
+}
+
+VECTORMATH_FORCE_INLINE const Vector3 rotate( const Quat &quat, const Vector3 &vec )
+{    __m128 qdata, vdata, product, tmp0, tmp1, tmp2, tmp3, wwww, qv, qw, res;
+    qdata = quat.get128();
+    vdata = vec.get128();
+    tmp0 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,0,2,1) );
+    tmp1 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,1,0,2) );
+    tmp2 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,1,0,2) );
+    tmp3 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,0,2,1) );
+    wwww = vec_splat( qdata, 3 );
+    qv = vec_mul( wwww, vdata );
+    qv = vec_madd( tmp0, tmp1, qv );
+    qv = vec_nmsub( tmp2, tmp3, qv );
+    product = vec_mul( qdata, vdata );
+    qw = vec_madd( vec_sld( qdata, qdata, 4 ), vec_sld( vdata, vdata, 4 ), product );
+    qw = vec_add( vec_sld( product, product, 8 ), qw );
+    tmp1 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,1,0,2) );
+    tmp3 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,0,2,1) );
+    res = vec_mul( vec_splat( qw, 0 ), qdata );
+    res = vec_madd( wwww, qv, res );
+    res = vec_madd( tmp0, tmp1, res );
+    res = vec_nmsub( tmp2, tmp3, res );
+    return Vector3( res );
+}
+
+VECTORMATH_FORCE_INLINE const Quat conj( const Quat &quat )
+{
+	VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0x80000000,0x80000000,0x80000000,0};
+    return Quat( vec_xor( quat.get128(), _mm_load_ps((float *)sw) ) );
+}
+
+VECTORMATH_FORCE_INLINE const Quat select( const Quat &quat0, const Quat &quat1, bool select1 )
+{
+    return select( quat0, quat1, boolInVec(select1) );
+}
+
+//VECTORMATH_FORCE_INLINE const Quat select( const Quat &quat0, const Quat &quat1, const boolInVec &select1 )
+//{
+//    return Quat( vec_sel( quat0.get128(), quat1.get128(), select1.get128() ) );
+//}
+
+VECTORMATH_FORCE_INLINE void loadXYZW(Quat& quat, const float* fptr)
+{
+#ifdef USE_SSE3_LDDQU
+	quat = Quat(	SSEFloat(_mm_lddqu_si128((const __m128i*)((float*)(fptr)))).m128		);
+#else
+	SSEFloat fl;
+	fl.f[0] = fptr[0];
+	fl.f[1] = fptr[1];
+	fl.f[2] = fptr[2];
+	fl.f[3] = fptr[3];
+    quat = Quat(	fl.m128);
+#endif
+    
+
+}
+
+VECTORMATH_FORCE_INLINE void storeXYZW(const Quat& quat, float* fptr)
+{
+	fptr[0] = quat.getX();
+	fptr[1] = quat.getY();
+	fptr[2] = quat.getZ();
+	fptr[3] = quat.getW();
+//    _mm_storeu_ps((float*)quat.get128(),fptr);
+}
+
+
+
+#ifdef _VECTORMATH_DEBUG
+
+VECTORMATH_FORCE_INLINE void print( const Quat &quat )
+{
+    union { __m128 v; float s[4]; } tmp;
+    tmp.v = quat.get128();
+    printf( "( %f %f %f %f )\n", tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );
+}
+
+VECTORMATH_FORCE_INLINE void print( const Quat &quat, const char * name )
+{
+    union { __m128 v; float s[4]; } tmp;
+    tmp.v = quat.get128();
+    printf( "%s: ( %f %f %f %f )\n", name, tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );
+}
+
+#endif
+
+} // namespace Aos
+} // namespace Vectormath
+
+#endif

Extras/PhysicsEffects/include/vecmath/sse/vecidx_aos.h

@@ -0,0 +1,80 @@
+/*
+   Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _VECTORMATH_VECIDX_AOS_H
+#define _VECTORMATH_VECIDX_AOS_H
+
+
+#include "floatInVec.h"
+
+namespace Vectormath {
+namespace Aos {
+
+//-----------------------------------------------------------------------------
+// VecIdx 
+// Used in setting elements of Vector3, Vector4, Point3, or Quat with the 
+// subscripting operator.
+//
+
+VM_ATTRIBUTE_ALIGNED_CLASS16 (class) VecIdx
+{
+private:
+   __m128 &ref;
+   int i;
+public:
+    inline VecIdx( __m128& vec, int idx ): ref(vec) { i = idx; }
+
+    // implicitly casts to float unless _VECTORMATH_NO_SCALAR_CAST defined
+    // in which case, implicitly casts to floatInVec, and one must call
+    // getAsFloat to convert to float.
+    //
+#ifdef _VECTORMATH_NO_SCALAR_CAST
+    inline operator floatInVec() const;
+    inline float getAsFloat() const;
+#else
+    inline operator float() const;
+#endif
+
+    inline float operator =( float scalar );
+    inline floatInVec operator =( const floatInVec &scalar );
+    inline floatInVec operator =( const VecIdx& scalar );
+    inline floatInVec operator *=( float scalar );
+    inline floatInVec operator *=( const floatInVec &scalar );
+    inline floatInVec operator /=( float scalar );
+    inline floatInVec operator /=( const floatInVec &scalar );
+    inline floatInVec operator +=( float scalar );
+    inline floatInVec operator +=( const floatInVec &scalar );
+    inline floatInVec operator -=( float scalar );
+    inline floatInVec operator -=( const floatInVec &scalar );
+};
+
+} // namespace Aos
+} // namespace Vectormath
+
+#endif

Extras/PhysicsEffects/include/vecmath/std/boolInVec.h

@@ -0,0 +1,238 @@
+/*
+   Copyright (C) 2006-2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _BOOLINVEC_SCALAR_H
+#define _BOOLINVEC_SCALAR_H
+
+#include <math.h>
+namespace Vectormath {
+
+class floatInVec;
+
+//--------------------------------------------------------------------------------------------------
+// boolInVec class
+//
+
+class boolInVec
+{
+private:
+    unsigned int mData;
+
+public:
+    // Default constructor; does no initialization
+    //
+    inline boolInVec( ) { };
+
+    // Construct from a value converted from float
+    //
+    inline boolInVec(floatInVec vec);
+
+    // Explicit cast from bool
+    //
+    explicit inline boolInVec(bool scalar);
+
+    // Explicit cast to bool
+    //
+    inline bool getAsBool() const;
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+    // Implicit cast to bool
+    //
+    inline operator bool() const;
+#endif
+
+    // Boolean negation operator
+    //
+    inline const boolInVec operator ! () const;
+
+    // Assignment operator
+    //
+    inline boolInVec& operator = (boolInVec vec);
+
+    // Boolean and assignment operator
+    //
+    inline boolInVec& operator &= (boolInVec vec);
+
+    // Boolean exclusive or assignment operator
+    //
+    inline boolInVec& operator ^= (boolInVec vec);
+
+    // Boolean or assignment operator
+    //
+    inline boolInVec& operator |= (boolInVec vec);
+
+};
+
+// Equal operator
+//
+inline const boolInVec operator == (boolInVec vec0, boolInVec vec1);
+
+// Not equal operator
+//
+inline const boolInVec operator != (boolInVec vec0, boolInVec vec1);
+
+// And operator
+//
+inline const boolInVec operator & (boolInVec vec0, boolInVec vec1);
+
+// Exclusive or operator
+//
+inline const boolInVec operator ^ (boolInVec vec0, boolInVec vec1);
+
+// Or operator
+//
+inline const boolInVec operator | (boolInVec vec0, boolInVec vec1);
+
+// Conditionally select between two values
+//
+inline const boolInVec select(boolInVec vec0, boolInVec vec1, boolInVec select_vec1);
+
+
+} // namespace Vectormath
+
+
+//--------------------------------------------------------------------------------------------------
+// boolInVec implementation
+//
+
+#include "floatInVec.h"
+
+namespace Vectormath {
+
+inline
+boolInVec::boolInVec(floatInVec vec)
+{
+    *this = (vec != floatInVec(0.0f));
+}
+
+inline
+boolInVec::boolInVec(bool scalar)
+{
+    mData = -(int)scalar;
+}
+
+inline
+bool
+boolInVec::getAsBool() const
+{
+    return (mData > 0);
+}
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+inline
+boolInVec::operator bool() const
+{
+    return getAsBool();
+}
+#endif
+
+inline
+const boolInVec
+boolInVec::operator ! () const
+{
+    return boolInVec(!mData);
+}
+
+inline
+boolInVec&
+boolInVec::operator = (boolInVec vec)
+{
+    mData = vec.mData;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator &= (boolInVec vec)
+{
+    *this = *this & vec;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator ^= (boolInVec vec)
+{
+    *this = *this ^ vec;
+    return *this;
+}
+
+inline
+boolInVec&
+boolInVec::operator |= (boolInVec vec)
+{
+    *this = *this | vec;
+    return *this;
+}
+
+inline
+const boolInVec
+operator == (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() == vec1.getAsBool());
+}
+
+inline
+const boolInVec
+operator != (boolInVec vec0, boolInVec vec1)
+{
+    return !(vec0 == vec1);
+}
+
+inline
+const boolInVec
+operator & (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() & vec1.getAsBool());
+}
+
+inline
+const boolInVec
+operator | (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() | vec1.getAsBool());
+}
+
+inline
+const boolInVec
+operator ^ (boolInVec vec0, boolInVec vec1)
+{
+    return boolInVec(vec0.getAsBool() ^ vec1.getAsBool());
+}
+
+inline
+const boolInVec
+select(boolInVec vec0, boolInVec vec1, boolInVec select_vec1)
+{
+    return (select_vec1.getAsBool() == 0) ? vec0 : vec1;
+}
+
+} // namespace Vectormath
+
+#endif // boolInVec_h

Extras/PhysicsEffects/include/vecmath/std/floatInVec.h

@@ -0,0 +1,357 @@
+/*
+   Copyright (C) 2006-2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _FLOATINVEC__SCALAR_H
+#define _FLOATINVEC__SCALAR_H
+
+#include <math.h>
+namespace Vectormath {
+
+class boolInVec;
+
+//--------------------------------------------------------------------------------------------------
+// floatInVec class
+//
+
+// A class representing a scalar float value contained in a vector register
+// This class does not support fastmath
+class floatInVec
+{
+private:
+    float mData;
+
+public:
+    // Default constructor; does no initialization
+    //
+    inline floatInVec( ) { };
+
+    // Construct from a value converted from bool
+    //
+    inline floatInVec(boolInVec vec);
+
+    // Explicit cast from float
+    //
+    explicit inline floatInVec(float scalar);
+
+    // Explicit cast to float
+    //
+    inline float getAsFloat() const;
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+    // Implicit cast to float
+    //
+    inline operator float() const;
+#endif
+
+    // Post increment (add 1.0f)
+    //
+    inline const floatInVec operator ++ (int);
+
+    // Post decrement (subtract 1.0f)
+    //
+    inline const floatInVec operator -- (int);
+
+    // Pre increment (add 1.0f)
+    //
+    inline floatInVec& operator ++ ();
+
+    // Pre decrement (subtract 1.0f)
+    //
+    inline floatInVec& operator -- ();
+
+    // Negation operator
+    //
+    inline const floatInVec operator - () const;
+
+    // Assignment operator
+    //
+    inline floatInVec& operator = (floatInVec vec);
+
+    // Multiplication assignment operator
+    //
+    inline floatInVec& operator *= (floatInVec vec);
+
+    // Division assignment operator
+    //
+    inline floatInVec& operator /= (floatInVec vec);
+
+    // Addition assignment operator
+    //
+    inline floatInVec& operator += (floatInVec vec);
+
+    // Subtraction assignment operator
+    //
+    inline floatInVec& operator -= (floatInVec vec);
+
+};
+
+// Multiplication operator
+//
+inline const floatInVec operator * (floatInVec vec0, floatInVec vec1);
+
+// Division operator
+//
+inline const floatInVec operator / (floatInVec vec0, floatInVec vec1);
+
+// Addition operator
+//
+inline const floatInVec operator + (floatInVec vec0, floatInVec vec1);
+
+// Subtraction operator
+//
+inline const floatInVec operator - (floatInVec vec0, floatInVec vec1);
+
+// Less than operator
+//
+inline const boolInVec operator < (floatInVec vec0, floatInVec vec1);
+
+// Less than or equal operator
+//
+inline const boolInVec operator <= (floatInVec vec0, floatInVec vec1);
+
+// Greater than operator
+//
+inline const boolInVec operator > (floatInVec vec0, floatInVec vec1);
+
+// Greater than or equal operator
+//
+inline const boolInVec operator >= (floatInVec vec0, floatInVec vec1);
+
+// Equal operator
+//
+inline const boolInVec operator == (floatInVec vec0, floatInVec vec1);
+
+// Not equal operator
+//
+inline const boolInVec operator != (floatInVec vec0, floatInVec vec1);
+
+// Conditionally select between two values
+//
+inline const floatInVec select(floatInVec vec0, floatInVec vec1, boolInVec select_vec1);
+
+
+} // namespace Vectormath
+
+
+//--------------------------------------------------------------------------------------------------
+// floatInVec implementation
+//
+
+#include "boolInVec.h"
+
+namespace Vectormath {
+
+inline
+floatInVec::floatInVec(boolInVec vec)
+{
+    mData = float(vec.getAsBool());
+}
+
+inline
+floatInVec::floatInVec(float scalar)
+{
+    mData = scalar;
+}
+
+inline
+float
+floatInVec::getAsFloat() const
+{
+    return mData;
+}
+
+#ifndef _VECTORMATH_NO_SCALAR_CAST
+inline
+floatInVec::operator float() const
+{
+    return getAsFloat();
+}
+#endif
+
+inline
+const floatInVec
+floatInVec::operator ++ (int)
+{
+    float olddata = mData;
+    operator ++();
+    return floatInVec(olddata);
+}
+
+inline
+const floatInVec
+floatInVec::operator -- (int)
+{
+    float olddata = mData;
+    operator --();
+    return floatInVec(olddata);
+}
+
+inline
+floatInVec&
+floatInVec::operator ++ ()
+{
+    *this += floatInVec(1.0f);
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator -- ()
+{
+    *this -= floatInVec(1.0f);
+    return *this;
+}
+
+inline
+const floatInVec
+floatInVec::operator - () const
+{
+    return floatInVec(-mData);
+}
+
+inline
+floatInVec&
+floatInVec::operator = (floatInVec vec)
+{
+    mData = vec.mData;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator *= (floatInVec vec)
+{
+    *this = *this * vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator /= (floatInVec vec)
+{
+    *this = *this / vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator += (floatInVec vec)
+{
+    *this = *this + vec;
+    return *this;
+}
+
+inline
+floatInVec&
+floatInVec::operator -= (floatInVec vec)
+{
+    *this = *this - vec;
+    return *this;
+}
+
+inline
+const floatInVec
+operator * (floatInVec vec0, floatInVec vec1)
+{
+    return floatInVec(vec0.getAsFloat() * vec1.getAsFloat());
+}
+
+inline
+const floatInVec
+operator / (floatInVec num, floatInVec den)
+{
+    return floatInVec(num.getAsFloat() / den.getAsFloat());
+}
+
+inline
+const floatInVec
+operator + (floatInVec vec0, floatInVec vec1)
+{
+    return floatInVec(vec0.getAsFloat() + vec1.getAsFloat());
+}
+
+inline
+const floatInVec
+operator - (floatInVec vec0, floatInVec vec1)
+{
+    return floatInVec(vec0.getAsFloat() - vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator < (floatInVec vec0, floatInVec vec1)
+{
+    return boolInVec(vec0.getAsFloat() < vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator <= (floatInVec vec0, floatInVec vec1)
+{
+    return !(vec0 > vec1);
+}
+
+inline
+const boolInVec
+operator > (floatInVec vec0, floatInVec vec1)
+{
+    return boolInVec(vec0.getAsFloat() > vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator >= (floatInVec vec0, floatInVec vec1)
+{
+    return !(vec0 < vec1);
+}
+
+inline
+const boolInVec
+operator == (floatInVec vec0, floatInVec vec1)
+{
+    return boolInVec(vec0.getAsFloat() == vec1.getAsFloat());
+}
+
+inline
+const boolInVec
+operator != (floatInVec vec0, floatInVec vec1)
+{
+    return !(vec0 == vec1);
+}
+
+inline
+const floatInVec
+select(floatInVec vec0, floatInVec vec1, boolInVec select_vec1)
+{
+    return (select_vec1.getAsBool() == 0) ? vec0 : vec1;
+}
+
+} // namespace Vectormath
+
+#endif // floatInVec_h

Extras/PhysicsEffects/include/vecmath/std/quat_aos.h

@@ -0,0 +1,446 @@
+/*
+   Copyright (C) 2006-2010 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   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 Sony Computer Entertainment 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 _VECTORMATH_QUAT_AOS_CPP_H
+#define _VECTORMATH_QUAT_AOS_CPP_H
+
+//-----------------------------------------------------------------------------
+// Definitions
+
+#ifndef _VECTORMATH_INTERNAL_FUNCTIONS
+#define _VECTORMATH_INTERNAL_FUNCTIONS
+
+#endif
+
+namespace Vectormath {
+namespace Aos {
+
+inline Quat::Quat( const Quat & quat )
+{
+    mX = quat.mX;
+    mY = quat.mY;
+    mZ = quat.mZ;
+    mW = quat.mW;
+}
+
+inline Quat::Quat( float _x, float _y, float _z, float _w )
+{
+    mX = _x;
+    mY = _y;
+    mZ = _z;
+    mW = _w;
+}
+
+inline Quat::Quat( const Vector3 & xyz, float _w )
+{
+    this->setXYZ( xyz );
+    this->setW( _w );
+}
+
+inline Quat::Quat( const Vector4 & vec )
+{
+    mX = vec.getX();
+    mY = vec.getY();
+    mZ = vec.getZ();
+    mW = vec.getW();
+}
+
+inline Quat::Quat( float scalar )
+{
+    mX = scalar;
+    mY = scalar;
+    mZ = scalar;
+    mW = scalar;
+}
+
+inline const Quat Quat::identity( )
+{
+    return Quat( 0.0f, 0.0f, 0.0f, 1.0f );
+}
+
+inline const Quat lerp( float t, const Quat & quat0, const Quat & quat1 )
+{
+    return ( quat0 + ( ( quat1 - quat0 ) * t ) );
+}
+
+inline const Quat slerp( float t, const Quat & unitQuat0, const Quat & unitQuat1 )
+{
+    Quat start;
+    float recipSinAngle, scale0, scale1, cosAngle, angle;
+    cosAngle = dot( unitQuat0, unitQuat1 );
+    if ( cosAngle < 0.0f ) {
+        cosAngle = -cosAngle;
+        start = ( -unitQuat0 );
+    } else {
+        start = unitQuat0;
+    }
+    if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
+        angle = acosf( cosAngle );
+        recipSinAngle = ( 1.0f / sinf( angle ) );
+        scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
+        scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
+    } else {
+        scale0 = ( 1.0f - t );
+        scale1 = t;
+    }
+    return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
+}
+
+inline const Quat squad( float t, const Quat & unitQuat0, const Quat & unitQuat1, const Quat & unitQuat2, const Quat & unitQuat3 )
+{
+    Quat tmp0, tmp1;
+    tmp0 = slerp( t, unitQuat0, unitQuat3 );
+    tmp1 = slerp( t, unitQuat1, unitQuat2 );
+    return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
+}
+
+inline void loadXYZW( Quat & quat, const float * fptr )
+{
+    quat = Quat( fptr[0], fptr[1], fptr[2], fptr[3] );
+}
+
+inline void storeXYZW( const Quat & quat, float * fptr )
+{
+    fptr[0] = quat.getX();
+    fptr[1] = quat.getY();
+    fptr[2] = quat.getZ();
+    fptr[3] = quat.getW();
+}
+
+inline Quat & Quat::operator =( const Quat & quat )
+{
+    mX = quat.mX;
+    mY = quat.mY;
+    mZ = quat.mZ;
+    mW = quat.mW;
+    return *this;
+}
+
+inline Quat & Quat::setXYZ( const Vector3 & vec )
+{
+    mX = vec.getX();
+    mY = vec.getY();
+    mZ = vec.getZ();
+    return *this;
+}
+
+inline const Vector3 Quat::getXYZ( ) const
+{
+    return Vector3( mX, mY, mZ );
+}
+
+inline Quat & Quat::setX( float _x )
+{
+    mX = _x;
+    return *this;
+}
+
+inline float Quat::getX( ) const
+{
+    return mX;
+}
+
+inline Quat & Quat::setY( float _y )
+{
+    mY = _y;
+    return *this;
+}
+
+inline float Quat::getY( ) const
+{
+    return mY;
+}
+
+inline Quat & Quat::setZ( float _z )
+{
+    mZ = _z;
+    return *this;
+}
+
+inline float Quat::getZ( ) const
+{
+    return mZ;
+}
+
+inline Quat & Quat::setW( float _w )
+{
+    mW = _w;
+    return *this;
+}
+
+inline float Quat::getW( ) const
+{
+    return mW;
+}
+
+inline Quat & Quat::setElem( int idx, float value )
+{
+    *(&mX + idx) = value;
+    return *this;
+}
+
+inline float Quat::getElem( int idx ) const
+{
+    return *(&mX + idx);
+}
+
+inline float & Quat::operator []( int idx )
+{
+    return *(&mX + idx);
+}
+
+inline float Quat::operator []( int idx ) const
+{
+    return *(&mX + idx);
+}
+
+inline const Quat Quat::operator +( const Quat & quat ) const
+{
+    return Quat(
+        ( mX + quat.mX ),
+        ( mY + quat.mY ),
+        ( mZ + quat.mZ ),
+        ( mW + quat.mW )
+    );
+}
+
+inline const Quat Quat::operator -( const Quat & quat ) const
+{
+    return Quat(
+        ( mX - quat.mX ),
+        ( mY - quat.mY ),
+        ( mZ - quat.mZ ),
+        ( mW - quat.mW )
+    );
+}
+
+inline const Quat Quat::operator *( float scalar ) const
+{
+    return Quat(
+        ( mX * scalar ),
+        ( mY * scalar ),
+        ( mZ * scalar ),
+        ( mW * scalar )
+    );
+}
+
+inline Quat & Quat::operator +=( const Quat & quat )
+{
+    *this = *this + quat;
+    return *this;
+}
+
+inline Quat & Quat::operator -=( const Quat & quat )
+{
+    *this = *this - quat;
+    return *this;
+}
+
+inline Quat & Quat::operator *=( float scalar )
+{
+    *this = *this * scalar;
+    return *this;
+}
+
+inline const Quat Quat::operator /( float scalar ) const
+{
+    return Quat(
+        ( mX / scalar ),
+        ( mY / scalar ),
+        ( mZ / scalar ),
+        ( mW / scalar )
+    );
+}
+
+inline Quat & Quat::operator /=( float scalar )
+{
+    *this = *this / scalar;
+    return *this;
+}
+
+inline const Quat Quat::operator -( ) const
+{
+    return Quat(
+        -mX,
+        -mY,
+        -mZ,
+        -mW
+    );
+}
+
+inline const Quat operator *( float scalar, const Quat & quat )
+{
+    return quat * scalar;
+}
+
+inline float dot( const Quat & quat0, const Quat & quat1 )
+{
+    float result;
+    result = ( quat0.getX() * quat1.getX() );
+    result = ( result + ( quat0.getY() * quat1.getY() ) );
+    result = ( result + ( quat0.getZ() * quat1.getZ() ) );
+    result = ( result + ( quat0.getW() * quat1.getW() ) );
+    return result;
+}
+
+inline float norm( const Quat & quat )
+{
+    float result;
+    result = ( quat.getX() * quat.getX() );
+    result = ( result + ( quat.getY() * quat.getY() ) );
+    result = ( result + ( quat.getZ() * quat.getZ() ) );
+    result = ( result + ( quat.getW() * quat.getW() ) );
+    return result;
+}
+
+inline float length( const Quat & quat )
+{
+    return ::sqrtf( norm( quat ) );
+}
+
+inline const Quat normalize( const Quat & quat )
+{
+    float lenSqr, lenInv;
+    lenSqr = norm( quat );
+    lenInv = ( 1.0f / sqrtf( lenSqr ) );
+    return Quat(
+        ( quat.getX() * lenInv ),
+        ( quat.getY() * lenInv ),
+        ( quat.getZ() * lenInv ),
+        ( quat.getW() * lenInv )
+    );
+}
+
+inline const Quat Quat::rotation( const Vector3 & unitVec0, const Vector3 & unitVec1 )
+{
+    float cosHalfAngleX2, recipCosHalfAngleX2;
+    cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f + dot( unitVec0, unitVec1 ) ) ) );
+    recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
+    return Quat( ( cross( unitVec0, unitVec1 ) * recipCosHalfAngleX2 ), ( cosHalfAngleX2 * 0.5f ) );
+}
+
+inline const Quat Quat::rotation( float radians, const Vector3 & unitVec )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( ( unitVec * s ), c );
+}
+
+inline const Quat Quat::rotationX( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( s, 0.0f, 0.0f, c );
+}
+
+inline const Quat Quat::rotationY( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( 0.0f, s, 0.0f, c );
+}
+
+inline const Quat Quat::rotationZ( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( 0.0f, 0.0f, s, c );
+}
+
+inline const Quat Quat::operator *( const Quat & quat ) const
+{
+    return Quat(
+        ( ( ( ( mW * quat.mX ) + ( mX * quat.mW ) ) + ( mY * quat.mZ ) ) - ( mZ * quat.mY ) ),
+        ( ( ( ( mW * quat.mY ) + ( mY * quat.mW ) ) + ( mZ * quat.mX ) ) - ( mX * quat.mZ ) ),
+        ( ( ( ( mW * quat.mZ ) + ( mZ * quat.mW ) ) + ( mX * quat.mY ) ) - ( mY * quat.mX ) ),
+        ( ( ( ( mW * quat.mW ) - ( mX * quat.mX ) ) - ( mY * quat.mY ) ) - ( mZ * quat.mZ ) )
+    );
+}
+
+inline Quat & Quat::operator *=( const Quat & quat )
+{
+    *this = *this * quat;
+    return *this;
+}
+
+inline const Vector3 rotate( const Quat & quat, const Vector3 & vec )
+{
+    float tmpX, tmpY, tmpZ, tmpW;
+    tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
+    tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
+    tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
+    tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
+    return Vector3(
+        ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
+        ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
+        ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
+    );
+}
+
+inline const Quat conj( const Quat & quat )
+{
+    return Quat( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
+}
+
+inline const Quat select( const Quat & quat0, const Quat & quat1, bool select1 )
+{
+    return Quat(
+        ( select1 )? quat1.getX() : quat0.getX(),
+        ( select1 )? quat1.getY() : quat0.getY(),
+        ( select1 )? quat1.getZ() : quat0.getZ(),
+        ( select1 )? quat1.getW() : quat0.getW()
+    );
+}
+
+#ifdef _VECTORMATH_DEBUG
+
+inline void print( const Quat & quat )
+{
+    printf( "( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
+}
+
+inline void print( const Quat & quat, const char * name )
+{
+    printf( "%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
+}
+
+#endif
+
+} // namespace Aos
+} // namespace Vectormath
+
+#endif