Anonymous avatar Anonymous committed 7a9702e

Comments (0)

Files changed (22)

Document/API List.txt

+class: pygame.physics.world
+
+class world(object)
+	####################
+	# methods
+
+	def _init_(self)
+	
+	def add_body(body)  #return body id
+	def remove_body(body)
+	
+	def add_joint(joint) #return joint id
+	def remove_joint(joint)
+	
+	def update() #return collision bodies list
+	#####################
+	#properties
+	
+	self.total_update_time
+	self.since_last_update_time
+	
+	
+class: pygame.physics.body
+
+class body(object)
+	####################
+	# methods
+
+	def _init_(self)
+
+
+	#####################
+	#properties
+	
+	self.mass
+	self.shape
+	self.position
+	self.angle
+	self.force
+	self.torque
+	self.linear_damping
+	self.angle_damping
+	self.resist         # surface property, for collision response by equation : sum(m*v) = -mul(resist)*sum(m*v')
+	
+	self.is_static
+	self.user_data   # user defined other game data
+	
+	
+
+	
+class pygame.physics.joint.joint_base
+(Here I want to explain why I design like this: it's an abstract class, users can write their own joint class by inheriting it, the world maintain joint list inside. Box2d also uses this way)
+
+
+class joint_base(object) 
+	
+	####################
+	# methods
+	def  _init_(self)
+	def  _init_(self, from, to)
+
+	#####################
+	#properties
+	
+	self.from_body
+	self.to_body
+	self.is_collide_connected
+	self.user_data   # user defined other game data
+	
+example joint: distance joint
+
+class pygame.physics.joint.joint_distance
+
+class joint_distance(joint)
+	####################
+	# methods
+	def  _init_(self)
+	def  _init_(self, from, to)
+
+	#####################
+	#properties
+	self.local_anchor1
+	self.local_anchor2
+	self.length
+	
+example code:
+
+	joint = new joint_distance(body1,body2)
+	joint.local_anchor1 = new vector2(0,0)
+	joint.local_anchor2 = new vector2(0,0)
+	joint.length = 10
+	world.add_joint(joint)
+	
+	
+##############################
+# classes for basic physics functions
+module : pygame.physics.basic
+	
+class aabb_box:
+    def collision_with(obb_box):
+
+class obb_box:
+    def collision_with(obb_box):
+    
+    
+class shape:
+    def get_aabb_box():
+    def get_obb_box():
+
+class Polygon(Shape) # override base functions
+class Circle(Shape) # override base functions
+
+
+###############################
+# Math classes 
+
+(Maybe it needs redesigned or we can use a exist python vector math library)
+
+module : pygame.physics.math
+
+class vector2
+	self.x
+	self.y
+	self.z
+	
+	def _init_(self)
+	def _init_(self,x,y,z)
+	
+class matrix2x2
+	self.x11
+	self.x12
+	self.x21
+	self.x22
+	
+	def _init_(self)
+	
+def add_vector2(vec1,vec2) # result = vec1 + vec2
+def sub_vector2(vec1,vec2) # result = vec1 - vec2
+def dot_vector2(vec1,vec2) # result = vec1 dot vec2  (result is a scalar)
+def cross_vector2(vec1,vec2) # result = vec1 cross vec2 (result is a vector)
+
+def add_matrix2x2(mat1,mat2)
+def sub_matrix2x2(mat1,mat2)
+def mul_matrix2x2(mat1,mat2)
+def mul_matrix2x2_vector2(mat,vec) # result = mat * vec  (result is a vec) , for transform and rotation
+
+
+
+2. pygame higher render module
+
+class physics_sprite(SomeKindOfSprite):
+    self.body  #Class Body
+    def update(self, Surface):
+        #do phy updates
+
+    def _init_(self, PhyGroup):
+    # here PhyGroup derive from Group
+    #or
+    #def _init_(self, group)
+    
+    
+class physics_group(group)
+    #physics render here
+    def update(self)
+    def _init(self)
+def phycollision(group,group)

Document/Physics algorithm.txt

+
+1.Simulation Step Algorithm
+	
+	Collision Detection and find all contacts
+	Find and build awake islands (An island contains bodys and joints which are connected together)
+		Depth first search algorithm to the body joints connect graph
+		when no body in the island is awake, the island is sleep 
+	Simulate awake islands
+		Free bodys simulation: Integrate velocities and apply damping.
+		Solve contacts and joints interatively
+		Update bodys positions
+		update body's states: whether it's sleep now. 
+	Post step
+		clear contact list
+		clear islands 
+		
+		
+

Document/ToDoList.txt

+1. Finish Pygame Physics Code Frame   ***DONE****
+2. Finish Test Code Frame   ****DONE****
+3. Algorithm For particle simulation   *****DONE*******
+4. Algorithm For Rigid Body simulation   
+5. Shape Defination , Collision Detection
+	Rect Shape					***DONE****
+	Circle Shape
+	Polygon Shape
+6. Joint Defination , Joint Solver
+	Distance Joint					***DONE****  
+7. Final Test and Python wrapper
+8. Pygame Physics Sprite Integration
+9. Demo

Pygame_Physics.sln

 EndProject
 Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Document", "Document", "{86532ADD-793B-4E68-A6C0-E911FD382476}"
 	ProjectSection(SolutionItems) = preProject
-		ToDoList.txt = ToDoList.txt
+		Document\Physics algorithm.txt = Document\Physics algorithm.txt
+		Document\ToDoList.txt = Document\ToDoList.txt
 	EndProjectSection
 EndProject
 Global

Pygame_Physics.vcproj

+<?xml version="1.0" encoding="gb2312"?>
+<VisualStudioProject
+	ProjectType="Visual C++"
+	Version="8.00"
+	Name="Pygame_Physics"
+	ProjectGUID="{DCBBE6BC-CD5F-4F51-B5C0-66715D9A72D8}"
+	RootNamespace="Pygame_Physics"
+	Keyword="Win32Proj"
+	>
+	<Platforms>
+		<Platform
+			Name="Win32"
+		/>
+	</Platforms>
+	<ToolFiles>
+	</ToolFiles>
+	<Configurations>
+		<Configuration
+			Name="Debug|Win32"
+			OutputDirectory="$(SolutionDir)$(ConfigurationName)"
+			IntermediateDirectory="$(ConfigurationName)"
+			ConfigurationType="4"
+			CharacterSet="1"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+			/>
+			<Tool
+				Name="VCCLCompilerTool"
+				Optimization="0"
+				AdditionalIncludeDirectories=".\include"
+				PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
+				MinimalRebuild="true"
+				BasicRuntimeChecks="3"
+				RuntimeLibrary="3"
+				UsePrecompiledHeader="0"
+				WarningLevel="3"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="4"
+			/>
+			<Tool
+				Name="VCManagedResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCPreLinkEventTool"
+			/>
+			<Tool
+				Name="VCLibrarianTool"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+			/>
+		</Configuration>
+		<Configuration
+			Name="Release|Win32"
+			OutputDirectory="$(SolutionDir)$(ConfigurationName)"
+			IntermediateDirectory="$(ConfigurationName)"
+			ConfigurationType="4"
+			CharacterSet="1"
+			WholeProgramOptimization="1"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+			/>
+			<Tool
+				Name="VCCLCompilerTool"
+				PreprocessorDefinitions="WIN32;NDEBUG;_LIB"
+				RuntimeLibrary="2"
+				UsePrecompiledHeader="0"
+				WarningLevel="3"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="3"
+			/>
+			<Tool
+				Name="VCManagedResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCPreLinkEventTool"
+			/>
+			<Tool
+				Name="VCLibrarianTool"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+			/>
+		</Configuration>
+	</Configurations>
+	<References>
+	</References>
+	<Files>
+		<Filter
+			Name="Source Files"
+			Filter="cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx"
+			UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"
+			>
+			<File
+				RelativePath=".\Src\pgAABBBox.c"
+				>
+			</File>
+			<File
+				RelativePath=".\Src\pgBodyObject.c"
+				>
+			</File>
+			<File
+				RelativePath=".\Src\pgCollision.c"
+				>
+			</File>
+			<File
+				RelativePath=".\Src\pgJointObject.c"
+				>
+			</File>
+			<File
+				RelativePath=".\Src\pgShapeObject.c"
+				>
+			</File>
+			<File
+				RelativePath=".\Src\pgVector2.c"
+				>
+			</File>
+			<File
+				RelativePath=".\Src\pgWorldObject.c"
+				>
+			</File>
+		</Filter>
+		<Filter
+			Name="Header Files"
+			Filter="h;hpp;hxx;hm;inl;inc;xsd"
+			UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}"
+			>
+			<File
+				RelativePath=".\include\pgAABBBox.h"
+				>
+			</File>
+			<File
+				RelativePath=".\include\pgBodyObject.h"
+				>
+			</File>
+			<File
+				RelativePath=".\include\pgCollision.h"
+				>
+			</File>
+			<File
+				RelativePath=".\include\pgJointObject.h"
+				>
+			</File>
+			<File
+				RelativePath=".\include\pgShapeObject.h"
+				>
+			</File>
+			<File
+				RelativePath=".\include\pgVector2.h"
+				>
+			</File>
+			<File
+				RelativePath=".\include\pgWorldObject.h"
+				>
+			</File>
+		</Filter>
+	</Files>
+	<Globals>
+	</Globals>
+</VisualStudioProject>
 	glLoadIdentity();									
 }
 
-void InitWorld()
+//============================================
+
+void TestBasic1Init()
+{
+	pgBodyObject* body;
+	pgJointObject* joint;
+	pgVector2 a1,a2;
+	PG_Set_Vector2(a1,0,0);
+	PG_Set_Vector2(a2,0,100);
+
+	s_world = PG_WorldNew();
+	s_world->fStepTime = 0.03;
+	body = PG_BodyNew();
+	PG_Set_Vector2(body->vecPosition,0,0)
+	PG_Set_Vector2(body->vecLinearVelocity,40,0)
+	PG_AddBodyToWorld(s_world,body);
+	
+	
+	joint = PG_DistanceJointNew(body,NULL,0,100,a1,a2);
+	PG_AddJointToWorld(s_world,joint);
+}
+
+void TestBasic2Init()
 {
 	pgBodyObject* body;
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
 	body = PG_BodyNew();
-	body->vecPosition.real = 0;
-	body->vecPosition.imag = 0;
-	body->vecLinearVelocity.real = 10;
+	PG_Set_Vector2(body->vecPosition,0,0)
+	PG_Set_Vector2(body->vecLinearVelocity,10,0)
 	PG_AddBodyToWorld(s_world,body);
 }
 
+//===============================================
+
+void InitWorld()
+{
+	TestBasic1Init();
+}
 
 int main (int argc, char** argv)
 {

Test/pgPhysicsRenderer.c

 #include "pgPhysicsRenderer.h"
+#include "pgShapeObject.h"
 #include <gl/glut.h>
 
 void PGT_RenderWorld(pgWorldObject* world)
 		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList),i));
 		PGT_RenderBody(body);
 	}
+
+	size = PyList_Size((PyObject*)(world->jointList));
+	for (i = 0;i < size;i++)
+	{
+		pgJointObject* joint = (pgJointObject*)(PyList_GetItem((PyObject*)(world->jointList),i));
+		PGT_RenderJoint(joint);
+	}
 }
 
 void PGT_RenderBody(pgBodyObject* body)
 {
-	glColor3f(1,0,0);
-	glPointSize(20);
-	glBegin(GL_POINTS);
-		glVertex2d(body->vecPosition.real,body->vecPosition.imag);
+	pgVector2 gp[4];
+	int i;
+	pgRectShape* rect = (pgRectShape*)body->shape;
+
+	for(i = 0; i < 4; ++i)
+		gp[i] = PG_GetGlobalCor(body, &(rect->point[i]));
+
+	glColor3f(1.f, 1.f, 0.f);
+	glLineWidth(2.f);
+	glBegin(GL_LINE_LOOP);
+	glVertex2d(gp[0].real, gp[0].imag);
+	glVertex2d(gp[1].real, gp[1].imag);
+	glVertex2d(gp[2].real, gp[2].imag);
+	glVertex2d(gp[3].real, gp[3].imag);
+	glVertex2d(gp[0].real, gp[0].imag);
 	glEnd();
-}
+	glLineWidth(1.f);
+}
+
+void PGT_RenderJoint(pgJointObject* joint)
+{
+	pgDistanceJoint* pj = (pgDistanceJoint*)joint;
+	glColor3f(1.f, 0.f, 0.f);
+	glLineWidth(2.f);
+	glBegin(GL_LINES);
+	glVertex2d(joint->body1->vecPosition.real,joint->body1->vecPosition.imag);
+	glVertex2d(pj->anchor2.real,pj->anchor2.imag);
+	glEnd();
+	glLineWidth(1.f);
+}

Test/pgPhysicsRenderer.h

 
 #include "pgBodyObject.h"
 #include "pgWorldObject.h"
+#include "pgJointObject.h"
 
 void PGT_RenderWorld(pgWorldObject* world);
 void PGT_RenderBody(pgBodyObject* body);
+void PGT_RenderJoint(pgJointObject* joint);
 
 #endif //_PYGAME_PHYSICS_RENDERER_

include/pgAABBBox.h

 #ifndef _PYGAME_MATH_AABBBOX_
 #define _PYGAME_MATH_AABBBOX_
 
-
-
 typedef struct _pgAABBBox{
-	double left,right,bottom,top;
+	double left, right, bottom, top;
 } pgAABBBox;
 
+pgAABBBox PG_GenAABB(double left, double right, double bottom, double top);
+
 #endif //_PYGAME_MATH_AABBBOX_

include/pgBodyObject.h

 #define _PYGAME_PHYSICS_BODY_
 
 
-#include <Python.h>
+#include "pgVector2.h"
 
 typedef struct _pgWorldObject pgWorldObject;
+typedef struct _pgShapeObject pgShapeObject;
 
 typedef struct _pgBodyObject{
 	PyObject_HEAD
 
 	double		fMass;
-	Py_complex	vecLinearVelocity;
+	pgVector2	vecLinearVelocity;
 	double		fAngleVelocity;
 
-	Py_complex	vecPosition;
+	pgVector2	vecPosition;
 	double		fRotation;
-	Py_complex	vecImpulse;
-	Py_complex	vecForce;
+	pgVector2	vecImpulse;
+	pgVector2	vecForce;
 	double		fTorque;
 
 	double		fRestitution;
 	double		fFriction;
+
+	pgShapeObject* shape;
+
 } pgBodyObject;
 
 pgBodyObject* PG_BodyNew();
 void	PG_BodyDestroy(pgBodyObject* body);
 
-void PG_FreeUpdateBody(pgWorldObject* world,pgBodyObject* body,double dt);
+void PG_FreeUpdateBodyVel(pgWorldObject* world, pgBodyObject* body, double dt);
+void PG_FreeUpdateBodyPos(pgWorldObject* world, pgBodyObject* body, double dt);
 
+pgVector2 PG_GetGlobalCor(pgBodyObject* body, pgVector2* local);
 
 #endif //_PYGAME_PHYSICS_BODY_

include/pgCollision.h

+#ifndef _PYGAME_PHYSICS_COLLISION_
+#define _PYGAME_PHYSICS_COLLISION_
+
+
+
+
+#endif

include/pgJointObject.h

 
 #include "pgBodyObject.h"
 
-typedef struct _pgJoint pgJoint;
+typedef struct _pgJointObject pgJointObject;
 
-typedef struct _pgJoint{
+typedef struct _pgJointObject{
 	PyObject_HEAD
 
 	pgBodyObject*	body1;
 	pgBodyObject*	body2;
-	bool	isCollideConnect;
-	void	(*SolveConstraint)(pgJoint* joint,double stepTime);
-} pgJoint;
+	int		isCollideConnect;
+	void	(*SolveConstraint)(pgJointObject* joint,double stepTime);
+	void	(*Destroy)(pgJointObject* joint);
+} pgJointObject;
+
+void PG_JointDestroy(pgJointObject* joint);
 
 typedef struct _pgDistanceJoint{
-	pgJoint		joint;
+	pgJointObject		joint;
+
 	double		distance;
-	Py_complex	anchor1,anchor2;
+	pgVector2	anchor1,anchor2;
 } pgDistanceJoint;
 
-
+pgJointObject* PG_DistanceJointNew(pgBodyObject* b1,pgBodyObject* b2,int bCollideConnect,double distance,pgVector2 a1,pgVector2 a2);
 
 #endif //_PYGAME_PHYSICS_JOINT_

include/pgShapeObject.h

 
 #include <Python.h>
 #include "pgAABBBox.h"
+#include "pgVector2.h"
 
 typedef struct _pgBodyObject pgBodyObject;
-
+typedef struct _pgShapeObject pgShapeObject;
 
 // shape base type
 typedef struct _pgShapeObject{
 	PyObject_HEAD
 
-	pgBodyObject*		body;
-	pgAABBBox	box;
+	pgAABBBox box;
+	pgBodyObject* body;
+	//pgVector2 centroid;
 
-	void (*DestroyShape)();
-	void (*UpdateAABBBox)();
+	//virtual functions
+	void (*Destroy)(pgShapeObject *shape);
+	int (*IsPointIn)(pgShapeObject* shape, pgVector2* point);
 } pgShapeObject;
 
 void	PG_ShapeDestroy(pgShapeObject* shape);
 
 //subclass type
+typedef struct _pgRectShape{
+	pgShapeObject shape;
 
-typedef struct _pgRectShape{
-	pgShapeObject		shape;
+	union
+	{
+		struct
+		{
+			pgVector2 point[4];
+		};
+		struct
+		{
+			pgVector2 bottomLeft, bottomRight, topRight, topLeft;
+		};
+	};
+	
+} pgRectShape;
 
-	pgAABBBox			rectBox;
-} pgPolygonShape;
+pgShapeObject*	PG_RectShapeNew(pgBodyObject* body, double width, double height, double seta);
 
-pgShapeObject*	PG_RectShapeNew(pgAABBBox rect);
 
-
-typedef struct _pgPolygonShape{
-	pgShapeObject		shape;
-
-	PyListObject*		vertexList;
-};
+//typedef struct _pgPolygonShape{
+//	pgShapeObject		shape;
+//
+//	PyListObject*		vertexList;
+//}pgPolygonShape;
 
 #endif //_PYGAME_PHYSICS_SHAPE_

include/pgVector2.h

+#ifndef _PYGAME_COMPLEX_EXTENSIONS_
+#define _PYGAME_COMPLEX_EXTENSIONS_
+
+
+#include <Python.h>
+
+#define ZERO_EPSILON 1e-7
+
+typedef Py_complex	pgVector2;
+#define PG_Set_Vector2(vec, x, y) {vec.real = x;vec.imag = y;}
+
+int is_zero(double num);
+int is_equal(double a, double b);
+
+double c_get_length_square(pgVector2 c);
+double c_get_length(pgVector2 c);
+Py_complex c_mul_complex_with_real(pgVector2 c,double d);
+void	c_normalize(pgVector2* pVec);
+double c_dot(pgVector2 a,pgVector2 b);
+double c_cross(pgVector2 a, pgVector2 b);
+void c_rotate(pgVector2* a, double seta);
+
+#endif //_PYGAME_COMPLEX_EXTENSIONS_

include/pgWorldObject.h

 #include "pgAABBBox.h"
 
 typedef struct _pgBodyObject pgBodyObject;
+typedef struct _pgJointObject pgJointObject;
 
 typedef struct _pgWorldObject 
 {
 void	PG_Update(pgWorldObject* world,double stepTime);
 void	PG_AddBodyToWorld(pgWorldObject* world,pgBodyObject* body);
 void	PG_RemoveBodyFromWorld(pgWorldObject* world,pgBodyObject* body);
+void	PG_AddJointToWorld(pgWorldObject* world,pgJointObject* joint);
+void	PG_RemoveJointFromWorld(pgWorldObject* world,pgJointObject* joint);
 
 #endif
+#include "pgAABBBox.h"
+
+pgAABBBox PG_GenAABB(double left, double right, double bottom, double top)
+{
+	pgAABBBox box;
+	box.left = left;
+	box.right = right;
+	box.bottom = bottom;
+	box.top = top;
+	return box;
+}

src/pgBodyObject.c

 #include "pgBodyObject.h"
 #include "pgWorldObject.h"
-#include "pgComplexExtensions.h"
+#include "pgVector2.h"
+#include "pgShapeObject.h"
 #include <structmember.h>
 
 
-void PG_FreeUpdateBody(pgWorldObject* world,pgBodyObject* body, double dt)
+void PG_FreeUpdateBodyVel(pgWorldObject* world,pgBodyObject* body, double dt)
 {
-	Py_complex totalVelAdd,totalPosAdd;
+	pgVector2 totalVelAdd;
 	double k;
 	totalVelAdd = c_sum(body->vecForce,world->vecGravity);
 	k = dt / body->fMass;
 	totalVelAdd = c_mul_complex_with_real(totalVelAdd,k);
 	body->vecLinearVelocity = c_sum(body->vecLinearVelocity,totalVelAdd);
+}
+
+void PG_FreeUpdateBodyPos(pgWorldObject* world,pgBodyObject* body,double dt)
+{
+	pgVector2 totalPosAdd;
 
 	totalPosAdd = c_mul_complex_with_real(body->vecLinearVelocity,dt);
 	body->vecPosition = c_sum(body->vecPosition,totalPosAdd);
 	body->fRestitution = 1.0;
 	body->fRotation = 0.0;
 	body->fTorque = 0.0;
-	body->vecForce.real = 0.0;
-	body->vecForce.imag = 0.0;
-	body->vecImpulse.real = 0.0;
-	body->vecImpulse.imag = 0.0;
-	body->vecLinearVelocity.real = 0.0;
-	body->vecLinearVelocity.imag = 0.0;
-	body->vecPosition.real = 0.0;
-	body->vecPosition.imag = 0.0;
+	PG_Set_Vector2(body->vecForce,0.0,0.0);
+	PG_Set_Vector2(body->vecImpulse,0.0,0.0);
+	PG_Set_Vector2(body->vecLinearVelocity,0.0,0.0);
+	PG_Set_Vector2(body->vecPosition,0.0,0.0);
+
+	//TODO: here just for testing, would be replaced by generic function
+	body->shape = PG_RectShapeNew(body, 20, 20, 0);
 }
 
 pgBodyObject* PG_BodyNew()
 	return op;
 }
 
-
+pgVector2 PG_GetGlobalCor(pgBodyObject* body, pgVector2* local)
+{
+	pgVector2 ans;
+	ans = *local;
+	c_rotate(&ans, body->fRotation);
+	ans = c_sum(ans, body->vecPosition);
+	return ans;
+}
 
 
 

src/pgCollision.c

+#include "pgCollision.h"

src/pgJointObject.c

+#include "pgJointObject.h"
+
+void PG_InitJointBase(pgJointObject* joint,pgBodyObject* b1,pgBodyObject* b2,int bCollideConnect)
+{
+	joint->body1 = b1;
+	joint->body2 = b2;
+	joint->isCollideConnect = bCollideConnect;
+	joint->SolveConstraint = NULL;
+}
+
+void PG_SolveDistanceJoint(pgJointObject* joint,double stepTime)
+{
+	pgVector2 vecL;
+	double lamda;
+	pgDistanceJoint* pJoint = (pgDistanceJoint*)joint;
+	if (joint->body1 && (!joint->body2))
+	{
+		vecL = c_diff(joint->body1->vecPosition,pJoint->anchor2);
+		c_normalize(&vecL);
+		lamda = -c_dot(vecL,joint->body1->vecLinearVelocity);
+		vecL = c_mul_complex_with_real(vecL,lamda);
+		joint->body1->vecLinearVelocity = c_sum(joint->body1->vecLinearVelocity,vecL);
+	}
+}
+
+pgJointObject* PG_DistanceJointNew(pgBodyObject* b1,pgBodyObject* b2,int bCollideConnect,double dist,pgVector2 a1,pgVector2 a2)
+{
+	pgDistanceJoint* pjoint = (pgDistanceJoint*)PyObject_MALLOC(sizeof(pgJointObject));
+	PG_InitJointBase(&(pjoint->joint), b1, b2, bCollideConnect);
+	pjoint->distance = dist;
+	pjoint->anchor1 = a1;
+	pjoint->anchor2 = a2;
+	pjoint->joint.SolveConstraint = PG_SolveDistanceJoint;
+	return (pgJointObject*)pjoint;
+}

src/pgShapeObject.c

+#include "pgShapeObject.h"
+#include "pgBodyObject.h"
+#include <string.h>
+#include <math.h>
+
+
+void PG_ShapeObjectInit(pgShapeObject* shape)
+{
+	//TODO: maybe these init methods are not suitable. need refined.
+	memset(&(shape->box), 0, sizeof(shape->box));
+	shape->Destroy = NULL;
+	shape->IsPointIn = NULL;
+	//shape->centroid.real = 0;
+	//shape->centroid.imag = 0;
+}
+
+
+void PG_RectShapeDestroy()
+{
+	//TODO: add destroy code
+}
+
+int PG_RectShapeIsPointIn(pgShapeObject* shape, pgVector2* point)
+{
+	pgRectShape* ps = (pgRectShape*)shape;
+	pgVector2 t1, t2;
+	pgVector2 gp[4];
+	double s1, s2;
+	int i;
+
+	t1 = c_diff(ps->bottomRight, ps->bottomLeft);
+	t2 = c_diff(ps->topLeft, ps->bottomLeft);
+	s1 = fabs(c_cross(t1, t2));
+	
+	s2 = 0;
+
+	for(i = 0; i < 4; ++i)
+		gp[i] = PG_GetGlobalCor(ps->shape.body, &(ps->point[i]));
+
+	for(i = 0; i < 4; ++i)
+	{
+		t1 = c_diff(gp[i], *point);
+		t2 = c_diff(gp[(i+1)%4], *point);
+		s2 += fabs(c_cross(t1, t2)); 
+	}
+
+	return is_equal(s1, s2);
+}
+
+pgShapeObject*	PG_RectShapeNew(pgBodyObject* body, double width, double height, double seta)
+{
+	int i;
+	pgRectShape* p = (pgRectShape*)PyObject_MALLOC(sizeof(pgRectShape));
+	
+	PG_ShapeObjectInit(&(p->shape));
+	p->shape.IsPointIn = PG_RectShapeIsPointIn;
+	
+	PG_Set_Vector2(p->bottomLeft, -width/2, -height/2);
+	PG_Set_Vector2(p->bottomRight, width/2, -height/2);
+	PG_Set_Vector2(p->topRight, width/2, height/2);
+	PG_Set_Vector2(p->topLeft, -width/2, height/2);
+	for(i = 0; i < 4; ++i)
+		c_rotate(&(p->point[i]), seta);
+
+	//p->shape.centroid = center;
+	p->shape.body = body;
+
+	return (pgShapeObject*)p;
+}
+
+#include "pgVector2.h"
+#include <assert.h>
+#include <math.h>
+
+int is_zero(double num)
+{
+	return fabs(num) < ZERO_EPSILON;
+}
+
+int is_equal(double a, double b)
+{
+	return is_zero(a - b);
+}
+
+double c_get_length_square(Py_complex c)
+{
+	double r;
+	r = c.real * c.real;
+	r += (c.imag * c.imag);
+	return r;
+}
+
+double c_get_length(Py_complex c)
+{
+	double r;
+	r = c.real * c.real;
+	r += (c.imag * c.imag);
+	return sqrt(r);
+}
+
+Py_complex c_mul_complex_with_real(Py_complex c,double d)
+{
+	Py_complex r;
+	r.real = c.real * d;
+	r.imag = c.imag * d;
+	return r;
+}
+
+void c_normalize(pgVector2* pVec)
+{
+	double l = c_get_length(*pVec);
+	assert(l > 0);
+	pVec->real /= l;
+	pVec->imag /= l;
+}
+
+double c_dot(pgVector2 a,pgVector2 b)
+{
+	return a.real * b.real + a.imag * b.imag;
+}
+
+double c_cross(pgVector2 a, pgVector2 b)
+{
+	return a.real*b.imag - a.imag*b.real;
+}
+
+void c_rotate(pgVector2* a, double seta)
+{
+	double x = a->real;
+	double y = a->imag;
+	a->real = x*cos(seta) - y*sin(seta);
+	a->imag = x*sin(seta) + y*cos(seta);
+}

src/pgWorldObject.c

 #include "pgWorldObject.h"
 #include "pgBodyObject.h"
+#include "pgJointObject.h"
 
 void _PG_FreeBodySimulation(pgWorldObject* world,double stepTime)
 {
 	for (i = 0;i < size;i++)
 	{
 		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList),i));
-		PG_FreeUpdateBody(world,body,stepTime);
+		PG_FreeUpdateBodyVel(world,body,stepTime);
 	}
 }
 
+void _PG_BodyCollisionDetection(pgWorldObject* world)
+{
+
+}
+
+void _PG_JointSolve(pgWorldObject* world,double stepTime)
+{
+	Py_ssize_t size = PyList_Size((PyObject*)(world->jointList));
+	Py_ssize_t i;
+	for (i = 0;i < size;i++)
+	{
+		pgJointObject* joint = (pgJointObject*)(PyList_GetItem((PyObject*)(world->jointList),i));
+		if (joint->SolveConstraint)
+		{
+			joint->SolveConstraint(joint,stepTime);
+		}
+	}
+}
+
+void _PG_BodyPositionUpdate(pgWorldObject* world,double stepTime)
+{
+	Py_ssize_t size = PyList_Size((PyObject*)(world->bodyList));
+	Py_ssize_t i;
+	for (i = 0;i < size;i++)
+	{
+		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList),i));
+		PG_FreeUpdateBodyPos(world,body,stepTime);
+	}
+}
+
+
 void PG_Update(pgWorldObject* world,double stepTime)
 {
-	_PG_FreeBodySimulation(world,stepTime);
+	_PG_FreeBodySimulation(world, stepTime);
+	_PG_BodyCollisionDetection(world);
+	_PG_JointSolve(world,stepTime);
+	_PG_BodyPositionUpdate(world, stepTime);
 }
 
 
 	
 }
 
+void PG_AddJointToWorld(pgWorldObject* world,pgJointObject* joint)
+{
+	PyList_Append((PyObject*)world->jointList,(PyObject*)joint);
+}
+
+void PG_RemoveJointFromWorld(pgWorldObject* world,pgJointObject* joint)
+{
+
+}
+
 void PG_WorldInit(pgWorldObject* world)
 {
 	world->bodyList = (PyListObject*)PyList_New(0);
 	world->jointList = (PyListObject*)PyList_New(0);
 	world->fDamping = 0.0;
 	world->fStepTime = 0.1;
+	PG_Set_Vector2(world->vecGravity,0.0,-50);
 	world->fTotalTime = 0.0;
-	world->vecGravity.real = 0.0;
-	world->vecGravity.imag = -9.8;
+
 }
 
 pgWorldObject* PG_WorldNew()
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.