Commits

Anonymous committed 8bc337b

添加了碰撞检测相关的代码
删除了bug多多的碰撞检测函数
新版本的碰撞响应算法已设计完毕,编码测试中

  • Participants
  • Parent commits eb8b6ea
  • Branches physics

Comments (0)

Files changed (17)

File Pygame_Physics.sln

 
-Microsoft Visual Studio Solution File, Format Version 9.00
-# Visual Studio 2005
+Microsoft Visual Studio Solution File, Format Version 10.00
+# Visual Studio 2008
 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Pygame_Physics", "Pygame_Physics.vcproj", "{DCBBE6BC-CD5F-4F51-B5C0-66715D9A72D8}"
 EndProject
 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Test", "Test\Test.vcproj", "{04AC4F22-E18F-4297-A791-A65126FAB7AA}"
 EndProject
 Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Document", "Document", "{86532ADD-793B-4E68-A6C0-E911FD382476}"
 	ProjectSection(SolutionItems) = preProject
+		Document\API List.txt = Document\API List.txt
 		Document\Physics algorithm.txt = Document\Physics algorithm.txt
 		Document\ToDoList.txt = Document\ToDoList.txt
 	EndProjectSection

File Pygame_Physics.vcproj

 <?xml version="1.0" encoding="gb2312"?>
 <VisualStudioProject
 	ProjectType="Visual C++"
-	Version="8.00"
+	Version="9.00"
 	Name="Pygame_Physics"
 	ProjectGUID="{DCBBE6BC-CD5F-4F51-B5C0-66715D9A72D8}"
 	RootNamespace="Pygame_Physics"
 	Keyword="Win32Proj"
+	TargetFrameworkVersion="131072"
 	>
 	<Platforms>
 		<Platform
 			<Tool
 				Name="VCCLCompilerTool"
 				Optimization="0"
-				AdditionalIncludeDirectories=".\include"
+				AdditionalIncludeDirectories="&quot;C:\Program Files\python\include&quot;;.\include"
 				PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
 				MinimalRebuild="true"
 				BasicRuntimeChecks="3"
 				RuntimeLibrary="3"
 				UsePrecompiledHeader="0"
 				WarningLevel="3"
-				Detect64BitPortabilityProblems="true"
+				Detect64BitPortabilityProblems="false"
 				DebugInformationFormat="4"
 			/>
 			<Tool

File Test/Test.vcproj

 <?xml version="1.0" encoding="gb2312"?>
 <VisualStudioProject
 	ProjectType="Visual C++"
-	Version="8.00"
+	Version="9.00"
 	Name="Test"
 	ProjectGUID="{04AC4F22-E18F-4297-A791-A65126FAB7AA}"
 	RootNamespace="Test"
 	Keyword="Win32Proj"
+	TargetFrameworkVersion="131072"
 	>
 	<Platforms>
 		<Platform
 			<Tool
 				Name="VCCLCompilerTool"
 				Optimization="0"
-				AdditionalIncludeDirectories="..\include"
+				AdditionalIncludeDirectories="&quot;C:\Program Files\python\include&quot;;..\include"
 				PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE;_CRT_TERMINATE_DEFINED"
 				MinimalRebuild="true"
 				BasicRuntimeChecks="3"
 				RuntimeLibrary="3"
 				UsePrecompiledHeader="0"
 				WarningLevel="3"
-				Detect64BitPortabilityProblems="true"
+				Detect64BitPortabilityProblems="false"
 				DebugInformationFormat="4"
 			/>
 			<Tool
 				Name="VCLinkerTool"
 				AdditionalDependencies="..\Debug\Pygame_Physics.lib"
 				LinkIncremental="2"
+				AdditionalLibraryDirectories="&quot;C:\Program Files\python\libs&quot;"
 				GenerateDebugInformation="true"
 				SubSystem="1"
+				RandomizedBaseAddress="1"
+				DataExecutionPrevention="0"
 				TargetMachine="1"
 			/>
 			<Tool
 				Name="VCAppVerifierTool"
 			/>
 			<Tool
-				Name="VCWebDeploymentTool"
-			/>
-			<Tool
 				Name="VCPostBuildEventTool"
 			/>
 		</Configuration>
 				SubSystem="1"
 				OptimizeReferences="2"
 				EnableCOMDATFolding="2"
+				RandomizedBaseAddress="1"
+				DataExecutionPrevention="0"
 				TargetMachine="1"
 			/>
 			<Tool
 				Name="VCAppVerifierTool"
 			/>
 			<Tool
-				Name="VCWebDeploymentTool"
-			/>
-			<Tool
 				Name="VCPostBuildEventTool"
 			/>
 		</Configuration>

File Test/pgPhysicsRenderer.c

 #include "pgPhysicsRenderer.h"
 #include "pgShapeObject.h"
+#include "pgAABBBox.h"
 #include <gl/glut.h>
 
+int RENDER_AABB = 1;
+
 void PGT_RenderWorld(pgWorldObject* world)
 {
 	Py_ssize_t size = PyList_Size((PyObject*)(world->bodyList));
 	}
 }
 
+void PGT_RenderAABB(pgBodyObject* body)
+{
+	pgVector2 p[4], gp[4];
+	int i;
+	pgAABBBox* box;
+
+	box = &(body->shape->box);
+	
+	PG_Set_Vector2(p[0], box->left, box->bottom);
+	PG_Set_Vector2(p[1], box->right, box->bottom);
+	PG_Set_Vector2(p[2], box->right, box->top);
+	PG_Set_Vector2(p[3], box->left, box->top);
+	for(i = 0; i < 4; ++i)
+		gp[i] = PG_GetGlobalPos(body, &p[i]);
+
+	glColor3f(0.f, 1.f, 1.f);
+	glEnable(GL_LINE_STIPPLE);
+	glLineWidth(1.f);
+	glLineStipple(1, 0xF0F0);
+	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();
+	glDisable(GL_LINE_STIPPLE);
+}
+
 void PGT_RenderBody(pgBodyObject* body)
 {
 	pgVector2 gp[4];
 	pgRectShape* rect = (pgRectShape*)body->shape;
 
 	for(i = 0; i < 4; ++i)
-		gp[i] = PG_GetGlobalCor(body, &(rect->point[i]));
+		gp[i] = PG_GetGlobalPos(body, &(rect->point[i]));
 
 	glColor3f(1.f, 1.f, 0.f);
 	glLineWidth(2.f);
 	glVertex2d(gp[0].real, gp[0].imag);
 	glEnd();
 	glLineWidth(1.f);
+
+	if(RENDER_AABB)
+		PGT_RenderAABB(body);
 }
 
 void PGT_RenderJoint(pgJointObject* joint)

File Test/pgPhysicsRenderer.h

 void PGT_RenderBody(pgBodyObject* body);
 void PGT_RenderJoint(pgJointObject* joint);
 
+extern int RENDER_AABB;
+
 #endif //_PYGAME_PHYSICS_RENDERER_

File include/pgAABBBox.h

 #ifndef _PYGAME_MATH_AABBBOX_
 #define _PYGAME_MATH_AABBBOX_
 
-//TODO: complete the AABBBox and ODBBox
+#include "pgVector2.h"
+
+typedef struct _pgAABBBox pgAABBBox;
+
+//TODO: complete the AABBBox
 typedef struct _pgAABBBox{
-	double left, right, bottom, top;
+	union
+	{
+		struct
+		{
+			double left, bottom, right, top;
+		};
+		struct
+		{
+			double from[2], to[2];
+		};
+	};
 } pgAABBBox;
 
 pgAABBBox PG_GenAABB(double left, double right, double bottom, double top);
+void PG_AABBExpandTo(pgAABBBox* box, pgVector2* p);
+void PG_AABBClear(pgAABBBox* box);
+int PG_IsOverlap(pgAABBBox* boxA, pgAABBBox* boxB);
 
 #endif //_PYGAME_MATH_AABBBOX_

File include/pgBodyObject.h

 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);
+//transform point local_p's position from body's local coordinate to the world's global one.
+//TODO: is the local coordinate necessary? anyway let it alone right now.
+pgVector2 PG_GetGlobalPos(pgBodyObject* body, pgVector2* local_p);
+
+//return the global velocity of a point p (on the rigid body)
+//(notice: here p is defined in the global coordinate)
+pgVector2 PG_GetVelocity(pgBodyObject* body, pgVector2* global_p);
+
+pgVector2 PG_GetVelocity1(pgVector2 r, double w);
+
+//return p_in_A
+pgVector2 PG_GetRelativePos(pgBodyObject* bodyA, pgBodyObject* bodyB, pgVector2* p_in_B);
 
 #endif //_PYGAME_PHYSICS_BODY_

File include/pgCollision.h

 #ifndef _PYGAME_PHYSICS_COLLISION_
 #define _PYGAME_PHYSICS_COLLISION_
 
+#include "pgBodyObject.h"
 
+typedef struct _pgContact
+{
+	pgVector2 pos;
+	pgVector2 normal;
+	double depth;
+	pgBodyObject* incBody; //incident rigid body
+	pgBodyObject* refBody; //reference rigid body
+}pgContact;
 
+enum pgCollisionType
+{
+	MOVING_AWAY,
+	RESTING,
+	MOVING_TOWARD
+};
 
-#endif
+#endif

File include/pgShapeObject.h

 
 	pgAABBBox box;
 	pgBodyObject* body;
-	//pgVector2 centroid;
 
 	//virtual functions
-	void (*Destroy)(pgShapeObject *shape);
+	void (*Destroy)(pgShapeObject* shape);
 	int (*IsPointIn)(pgShapeObject* shape, pgVector2* point);
+	void (*UpdateAABB)(pgShapeObject* shape);
 } pgShapeObject;
 
 
-void	PG_ShapeDestroy(pgShapeObject* shape);
+void	PG_ShapeObjectDestroy(pgShapeObject* shape);
 
 //subclass type
 typedef struct _pgRectShape{
 //	PyListObject*		vertexList;
 //}pgPolygonShape;
 
-#endif //_PYGAME_PHYSICS_SHAPE_
+#endif //_PYGAME_PHYSICS_SHAPE_

File include/pgVector2.h

 
 
 #include <Python.h>
+#include <math.h>
 
 #define ZERO_EPSILON 1e-7
 
+#ifndef M_PI
+#define M_PI 3.1415926535897932384626433832795
+#endif
+
+
 typedef Py_complex	pgVector2;
-#define PG_Set_Vector2(vec, x, y) {vec.real = x;vec.imag = y;}
+#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);
 
+#define MAX(x, y) ( ((x) > (y)) ? (x) : (y) )
+#define MIN(x, y) ( ((x) < (y)) ? (x) : (y) )
+
 double c_get_length_square(pgVector2 c);
 double c_get_length(pgVector2 c);
 Py_complex c_mul_complex_with_real(pgVector2 c,double d);

File src/pgAABBBox.c

 #include "pgAABBBox.h"
+#include <float.h>
 
 
 pgAABBBox PG_GenAABB(double left, double right, double bottom, double top)
 	box.top = top;
 	return box;
 }
+
+void PG_AABBClear(pgAABBBox* box)
+{
+	box->left = DBL_MAX;
+	box->bottom = DBL_MAX;
+	box->top = -DBL_MAX;
+	box->right = -DBL_MAX;
+}
+
+void PG_AABBExpandTo(pgAABBBox* box, pgVector2* p)
+{
+	box->left = MIN(box->left, p->real);
+	box->right = MAX(box->right, p->real);
+	box->bottom = MIN(box->bottom, p->imag);
+	box->top = MAX(box->top, p->imag);
+}
+
+int PG_IsOverlap(pgAABBBox* boxA, pgAABBBox* boxB)
+{
+	double from_x, from_y, to_x, to_y;
+	from_x = MAX(boxA->left, boxB->left);
+	from_y = MAX(boxA->bottom, boxB->bottom);
+	to_x = MIN(boxA->right, boxB->right);
+	to_y = MIN(boxA->top, boxB->top);
+	return from_x <= to_x && from_y <= to_y;
+}

File src/pgBodyObject.c

 
 	totalPosAdd = c_mul_complex_with_real(body->vecLinearVelocity,dt);
 	body->vecPosition = c_sum(body->vecPosition,totalPosAdd);
+	body->shape->UpdateAABB(body->shape);
 }
 
 void PG_BodyInit(pgBodyObject* body)
 
 pgBodyObject* PG_BodyNew()
 {
-	//pgBodyObject* op;
-	//op = (pgBodyObject*)PyObject_MALLOC(sizeof(pgBodyObject));
-	//PG_BodyInit(op);
-	//return op;
 	return (pgBodyObject*) _PG_BodyNew(&pgBodyType, NULL, NULL);
 }
 
-pgVector2 PG_GetGlobalCor(pgBodyObject* body, pgVector2* local)
+pgVector2 PG_GetGlobalPos(pgBodyObject* body, pgVector2* local_p)
 {
 	pgVector2 ans;
-	ans = *local;
+	
+	ans = *local_p;
 	c_rotate(&ans, body->fRotation);
 	ans = c_sum(ans, body->vecPosition);
+
 	return ans;
 }
 
+pgVector2 PG_GetRelativePos(pgBodyObject* bodyA, pgBodyObject* bodyB, pgVector2* p_in_B)
+{
+	pgVector2 trans, p_in_A;
+	double rotate;
+	
+	trans = c_diff(bodyB->vecPosition, bodyA->vecPosition);
+	rotate = bodyA->fRotation - bodyB->fRotation;
+	p_in_A = *p_in_B;
+	c_rotate(&p_in_A, rotate);
+	p_in_A = c_sum(p_in_A, trans);
+	
+	return p_in_A;
+}
 
+pgVector2 PG_GetVelocity1(pgVector2 r, double w)
+{
+	pgVector2 v;
+	double r_len, v_len;
+
+	r_len = c_get_length(r);
+	if(is_zero(r_len))
+	{
+		v.imag = v.real = 0;
+	}
+	else
+	{
+		r.real /= r_len;
+		r.imag /= r_len;
+		v_len = fabs(r_len*w);
+		r.real *= v_len;
+		r.imag *= v_len;
+		if(w > 0) //counter-clock wise
+		{	
+			v.real = -r.imag;
+			v.imag = r.real;
+		}
+		else
+		{
+			v.real = r.imag;
+			v.imag = -r.real;
+		}
+	}
+
+	return v;
+}
+
+pgVector2 PG_GetVelocity(pgBodyObject* body, pgVector2* global_p)
+{	
+	//get rotate radius vector r
+	pgVector2 r = c_diff(*global_p, body->vecPosition);
+	return PG_GetVelocity1(r, body->fRotation);
+}
 
 
 //static PyMemberDef Body_members[] = {

File src/pgCollision.c

-#include "pgCollision.h"
+#include "pgCollision.h"
+#include "pgAABBBox.h"
+
+// We borrow this graph from Box2DLite
+// Box vertex and edge numbering:
+//
+//        ^ y
+//        |
+//        e1
+//   v2 ----- v1
+//    |        |
+// e2 |        | e4  --> x
+//    |        |
+//   v3 ----- v4
+//        e3
+
+
+//TODO: add rest contact
+
+
+// Apply Liang-Barskey clip on a AABB box
+// (p1, p2) is the input line segment to be clipped (note: it's a 2d vector)
+// (ans_p1, ans_p2) is the output line segment
+// TODO: tone Liang-Barskey clip
+int _LiangBarskey_Internal(double p, double q, double* u1, double* u2)
+{
+	if(is_zero(p) && q < 0) return 0;
+	
+	if(p < 0)
+		*u1 = MAX(*u1, q/p);
+	else
+		*u2 = MIN(*u2, q/p);
+
+	return 1; 
+}
+
+int _PG_LiangBarskey(pgAABBBox* box, pgVector2* p1, pgVector2* p2, pgVector2* ans_p1, pgVector2* ans_p2)
+{
+	pgVector2 dp;
+	double u1, u2;
+
+	u1 = 0.f;
+	u2 = 1.f;
+	dp = c_diff(*p2, *p1);	//dp = p2 - p1
+
+	if(!_LiangBarskey_Internal(-dp.real, p1->real - box->left, &u1, &u2)) return 0;
+	if(!_LiangBarskey_Internal(dp.real, box->right - p1->real, &u1, &u2)) return 0;
+	if(!_LiangBarskey_Internal(-dp.imag, p1->imag - box->bottom, &u1, &u2)) return 0;
+	if(!_LiangBarskey_Internal(dp.imag, box->top - p1->imag, &u1, &u2)) return 0;
+
+	if(u1 > u2) return 0;
+
+	*ans_p1 = c_sum(*p1, c_mul_complex_with_real(dp, u1)); //ans_p1 = p1 + u1*dp
+	*ans_p2 = c_sum(*p2, c_mul_complex_with_real(dp, u2)); //ans_p2 = p2 + u2*dp;
+
+	return 1;
+}
+
+pgContact* pg_BuildContact(pgBodyObject* bodyA, pgBodyObject* bodyB)
+{
+	//clipping
+	//find contact points
+	//find the (proper) normal
+
+	return NULL;
+}
+
+
+

File src/pgJointObject.c

 	pgDistanceJoint* pJoint = (pgDistanceJoint*)joint;
 	if (joint->body1 && (!joint->body2))
 	{
-		vecL = c_diff(joint->body1->vecPosition,pJoint->anchor2);
+		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);
+		lamda = -c_dot(vecL, joint->body1->vecLinearVelocity);
+		vecL = c_mul_complex_with_real(vecL, lamda);
+		joint->body1->vecLinearVelocity = c_sum(joint->body1->vecLinearVelocity, vecL);
 		return;
 	}
 
 	if(joint->body1 && joint->body2)
 	{
-		vecL = c_diff(joint->body1->vecPosition,joint->body2->vecPosition);
+		vecL = c_diff(joint->body1->vecPosition, joint->body2->vecPosition);
 		c_normalize(&vecL);
-		cosTheta1V = c_dot(vecL,joint->body1->vecLinearVelocity);
-		cosTheta2V = c_dot(vecL,joint->body2->vecLinearVelocity);
+		cosTheta1V = c_dot(vecL, joint->body1->vecLinearVelocity);
+		cosTheta2V = c_dot(vecL, joint->body2->vecLinearVelocity);
 		lamda = cosTheta1V - cosTheta2V;
 		mk = joint->body1->fMass * joint->body2->fMass / (joint->body1->fMass + joint->body2->fMass);
 		lamda *= mk;
-		impuseAdd = c_mul_complex_with_real(vecL,lamda);
-		v1Add = c_div_complex_with_real(impuseAdd,joint->body1->fMass);
-		v2Add = c_div_complex_with_real(impuseAdd,joint->body2->fMass);
+		impuseAdd = c_mul_complex_with_real(vecL, lamda);
+		v1Add = c_div_complex_with_real(impuseAdd, joint->body1->fMass);
+		v2Add = c_div_complex_with_real(impuseAdd, joint->body2->fMass);
 		/*joint->body1->vecLinearVelocity = c_sum(joint->body1->vecLinearVelocity,v1Add);
 		joint->body2->vecLinearVelocity = c_diff(joint->body2->vecLinearVelocity,v2Add);*/
-		joint->body1->vecLinearVelocity = c_diff(joint->body1->vecLinearVelocity,v1Add);
-		joint->body2->vecLinearVelocity = c_sum(joint->body2->vecLinearVelocity,v2Add);
+		joint->body1->vecLinearVelocity = c_diff(joint->body1->vecLinearVelocity, v1Add);
+		joint->body2->vecLinearVelocity = c_sum(joint->body2->vecLinearVelocity, v2Add);
 		return;
 	}
 }
 
 pgJointObject* PG_DistanceJointNew(pgBodyObject* b1,pgBodyObject* b2,int bCollideConnect,double dist,pgVector2 a1,pgVector2 a2)
 {
-	pgDistanceJoint* pjoint = (pgDistanceJoint*)PyObject_MALLOC(sizeof(pgDistanceJoint));
+	pgDistanceJoint* pjoint = (pgDistanceJoint*)PyObject_MALLOC(sizeof(pgDistanceJoint));	
 	PG_InitJointBase(&(pjoint->joint), b1, b2, bCollideConnect);
 	pjoint->distance = dist;
 	pjoint->anchor1 = a1;

File src/pgShapeObject.c

 
 void PG_ShapeObjectInit(pgShapeObject* shape)
 {
-	//TODO: maybe these init methods are not suitable. need refined.
+	//TODO: maybe these init methods are not suitable. needing refined.
 	memset(&(shape->box), 0, sizeof(shape->box));
 	shape->Destroy = NULL;
 	shape->IsPointIn = NULL;
-	//shape->centroid.real = 0;
-	//shape->centroid.imag = 0;
 }
 
 void PG_ShapeObjectDestroy(pgShapeObject* shape)
 	s2 = 0;
 
 	for(i = 0; i < 4; ++i)
-		gp[i] = PG_GetGlobalCor(ps->shape.body, &(ps->point[i]));
+		gp[i] = PG_GetGlobalPos(ps->shape.body, &(ps->point[i]));
 
 	for(i = 0; i < 4; ++i)
 	{
 	return is_equal(s1, s2);
 }
 
+void PG_RectShapeUpdateAABB(pgShapeObject* rectShape)
+{
+	int i;
+	pgRectShape *p = (pgRectShape*)rectShape;
+	PG_AABBClear(&(p->shape.box));
+	for(i = 0; i < 4; ++i)
+		PG_AABBExpandTo(&(p->shape.box), &(p->point[i]));
+}
+
 void PG_RectShapeDestroy(pgShapeObject* rectShape)
 {
 	PyObject_Free((pgRectShape*)rectShape);
 	PG_ShapeObjectInit(&(p->shape));
 	p->shape.IsPointIn = PG_RectShapeIsPointIn;
 	p->shape.Destroy = PG_RectShapeDestroy;
+	p->shape.UpdateAABB = PG_RectShapeUpdateAABB;
 
 	PG_Set_Vector2(p->bottomLeft, -width/2, -height/2);
 	PG_Set_Vector2(p->bottomRight, 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;

File src/pgVector2.c

 #include "pgVector2.h"
 #include <assert.h>
-#include <math.h>
 
 int is_zero(double num)
 {

File src/pgWorldObject.c

 
 }
 
+
+
+
+
+
 void PG_WorldInit(pgWorldObject* world)
 {
 	world->bodyList = (PyListObject*)PyList_New(0);
 
 	Py_INCREF(&pgWorldType);
 	PyModule_AddObject(m, "world", (PyObject *)&pgWorldType);
-}
+}