void PhysicsDebugDraw::drawShape(PhysicsShape& shape)
{
const Color4F fillColor(1.0f, 0.0f, 0.0f, 0.3f);
const Color4F outlineColor(1.0f, 0.0f, 0.0f, 1.0f);
for (auto it = shape._cpShapes.begin(); it != shape._cpShapes.end(); ++it)
{
cpShape *subShape = *it;
switch ((*it)->klass_private->type)
{
case CP_CIRCLE_SHAPE:
{
float radius = PhysicsHelper::cpfloat2float(cpCircleShapeGetRadius(subShape));
Vec2 centre = PhysicsHelper::cpv2point(cpBodyGetPos(cpShapeGetBody(subShape)));
Vec2 offset = PhysicsHelper::cpv2point(cpCircleShapeGetOffset(subShape));
Vec2 rotation(PhysicsHelper::cpv2point(cpBodyGetRot(cpShapeGetBody(subShape))));
centre += offset.rotate(rotation);
static const int CIRCLE_SEG_NUM = 12;
Vec2 seg[CIRCLE_SEG_NUM] = {};
for (int i = 0; i < CIRCLE_SEG_NUM; ++i)
{
float angle = (float)i * M_PI / (float)CIRCLE_SEG_NUM * 2.0f;
Vec2 d(radius * cosf(angle), radius * sinf(angle));
seg[i] = centre + d;
}
_drawNode->drawPolygon(seg, CIRCLE_SEG_NUM, fillColor, 1, outlineColor);
break;
}
case CP_SEGMENT_SHAPE:
{
cpSegmentShape *seg = (cpSegmentShape *)subShape;
_drawNode->drawSegment(PhysicsHelper::cpv2point(seg->ta),
PhysicsHelper::cpv2point(seg->tb),
PhysicsHelper::cpfloat2float(seg->r==0 ? 1 : seg->r), outlineColor);
break;
}
case CP_POLY_SHAPE:
{
cpPolyShape* poly = (cpPolyShape*)subShape;
int num = poly->numVerts;
Vec2* seg = new (std::nothrow) Vec2[num];
PhysicsHelper::cpvs2points(poly->tVerts, seg, num);
_drawNode->drawPolygon(seg, num, fillColor, 1.0f, outlineColor);
delete[] seg;
break;
}
default:
break;
}
}
}
static cpBool bulletPlaneBeginFunc(cpArbiter *arb, cpSpace *space, void*) {
cpShape *a,*b;
cpArbiterGetShapes(arb, &a,&b);
cpBody *targetBody = cpShapeGetBody(b);
if (((Bullet *)cpBodyGetUserData(cpShapeGetBody(a)))->player()->body() != targetBody) {
printf("Hit plane\n");
Player * player = (Player *)(cpBodyGetUserData(targetBody));
player->hurt(200);
cpSpaceAddPostStepCallback( space, (cpPostStepFunc)ammoFree, a, NULL);
return true;
}
return false;
}
// adds an asteroid shape to the cpSpace
cpBody * core_add_new_asteroid ( cpSpace *space, const int type, const double p1x, const double p1y, const double p2x, const double p2y, Color *color, const double orientation, const double friction, const double elasticity, const double density, const int index, cpVect impulse, cpVect offset ) {
cpShape * shape;
if ( type == CIRCLE_TYPE ) {
shape = core_add_new_shape ( space, type, p1x, p1y, p2x, p2y, color, orientation, friction, elasticity, density, index );
} else {
shape = core_add_single_segment_shape ( space, p1x, p1y, p2x, p2y, color, friction, elasticity, density, index );
} if ( shape == NULL ) return NULL;
DrawShapeInfo * dsi = (DrawShapeInfo *) shape->data;
dsi->space_shape_type = 1;
// set collision type
cpShapeSetCollisionType ( shape, ASTEROID_COLLISION_TYPE );
// apply impulse
cpBody *body = cpShapeGetBody ( shape );
impulse = cpv ( impulse.x * IMPULSE_MULTIPLIER, impulse.y * IMPULSE_MULTIPLIER );
cpBodyApplyImpulse ( body, impulse, offset );
return body;
}
void ws::game::Scene::draw(sf::RenderWindow& window) {
ws::components::Renderable::setGlobalRenderTarget(&window);
auto groundPos = cpBodyGetPos(cpShapeGetBody(ground));
auto rect = sf::RectangleShape(sf::Vector2f(400, 10));
//std::cout << "Ground Position [" << groundPos.x << "," << groundPos.y << "]" << std::endl;
cpVect screenSize = cpv(800,600);
cpVect screenPos = groundPos;
//screenPos = cpvsub(cpvmult(screenSize, 0.5), groundPos);
rect.setPosition(sf::Vector2f(screenSize.x - screenPos.x-200, screenSize.y - screenPos.y));
rect.setFillColor(sf::Color::Green);
window.draw(rect);
// Renderable
for( auto object : objects )
{
auto renderables = object->getAllComponentsOfType<ws::components::Renderable>();
for( auto renderable : renderables ) {
if( renderable != nullptr )
{
renderable->render();
}
}
}
}
void PhysicsDebugDraw::drawShape(PhysicsShape& shape)
{
for (auto it = shape._info->getShapes().begin(); it != shape._info->getShapes().end(); ++it)
{
cpShape *subShape = *it;
switch ((*it)->klass_private->type)
{
case CP_CIRCLE_SHAPE:
{
float radius = PhysicsHelper::cpfloat2float(cpCircleShapeGetRadius(subShape));
Point centre = PhysicsHelper::cpv2point(cpBodyGetPos(cpShapeGetBody(subShape)))
+ PhysicsHelper::cpv2point(cpCircleShapeGetOffset(subShape));
static const int CIRCLE_SEG_NUM = 12;
Point seg[CIRCLE_SEG_NUM] = {};
for (int i = 0; i < CIRCLE_SEG_NUM; ++i)
{
float angle = (float)i * M_PI / (float)CIRCLE_SEG_NUM * 2.0f;
Point d(radius * cosf(angle), radius * sinf(angle));
seg[i] = centre + d;
}
_drawNode->drawPolygon(seg, CIRCLE_SEG_NUM, Color4F(1.0f, 0.0f, 0.0f, 0.3f), 1, Color4F(1, 0, 0, 1));
break;
}
case CP_SEGMENT_SHAPE:
{
cpSegmentShape *seg = (cpSegmentShape *)subShape;
_drawNode->drawSegment(PhysicsHelper::cpv2point(seg->ta),
PhysicsHelper::cpv2point(seg->tb),
PhysicsHelper::cpfloat2float(seg->r==0 ? 1 : seg->r), Color4F(1, 0, 0, 1));
break;
}
case CP_POLY_SHAPE:
{
cpPolyShape* poly = (cpPolyShape*)subShape;
int num = poly->numVerts;
Point* seg = new Point[num];
PhysicsHelper::cpvs2points(poly->tVerts, seg, num);
_drawNode->drawPolygon(seg, num, Color4F(1.0f, 0.0f, 0.0f, 0.3f), 1.0f, Color4F(1.0f, 0.0f, 0.0f, 1.0f));
delete[] seg;
break;
}
default:
break;
}
}
}
/* Mouse handling is a bit tricky. We want the user to move
* tiles using the mouse but because tiles are dynamic bodies
* managed by Chipmunk2D, we cannot directly control them.
* This is resolved by creating a pivot joint between an
* invisible mouse body that we can control and the tile body
* that we cannot directly control.
*/
static void apply_mouse_motion(struct state* state)
{
struct mouse m;
update_mouse(&m);
int w, h;
get_screen_size(&w, &h);
int x = m.x_position * w;
int y = m.y_position * h;
cpVect mouse_pos = cpv(x, y);
cpVect new_point =
cpvlerp(cpBodyGetPosition(state->mouse_body), mouse_pos, 0.25f);
cpBodySetVelocity(
state->mouse_body,
cpvmult(cpvsub(new_point, cpBodyGetPosition(state->mouse_body)),
60.0f));
cpBodySetPosition(state->mouse_body, new_point);
if (m.left_click && state->mouse_joint == NULL) {
cpFloat radius = 5.0;
cpPointQueryInfo info = { 0 };
cpShape* shape = cpSpacePointQueryNearest(state->space, mouse_pos,
radius, GRAB_FILTER, &info);
if (shape && cpBodyGetMass(cpShapeGetBody(shape)) < INFINITY) {
cpVect nearest = (info.distance > 0.0f ? info.point : mouse_pos);
cpBody* body = cpShapeGetBody(shape);
state->mouse_joint =
cpPivotJointNew2(state->mouse_body, body, cpvzero,
cpBodyWorldToLocal(body, nearest));
cpConstraintSetMaxForce(state->mouse_joint, 5000000.0f);
cpConstraintSetErrorBias(state->mouse_joint,
cpfpow(1.0f - 0.15f, 60.0f));
cpSpaceAddConstraint(state->space, state->mouse_joint);
}
}
if (m.left_click == false && state->mouse_joint != NULL) {
cpSpaceRemoveConstraint(state->space, state->mouse_joint);
cpConstraintFree(state->mouse_joint);
state->mouse_joint = NULL;
}
}
//Post-step: Ammo free
static void ammoFree( cpSpace *space, cpShape *shape, void *unused) {
cpBody *body = cpShapeGetBody(shape);
Bullet *ammo = (Bullet*)cpBodyGetUserData(body);
//cpVect pos = cpBodyGetPosition(Body);
//printf("body->p: %f %f \n", Body->p.x, Body->p.y);
//printf("Position: %f %f \n",pos.x,pos.y);
//ammo->explosion(Body->p.x - 59, Body->p.y - 59);
// for(int i=0; i < EXPLOSION_MAXIMUM_SPRITES; i++)
// if( !explosionCheck[i] )
// explosionSet(i, Body->p.x - 59, Body->p.y - 59);
Explosion(body->p.x - 59, body->p.y -59);
printf("Post step free");
ammo->destroy();
}
void handle_shape(cpShape *shape, void *data) {
cpBody *body = cpShapeGetBody(shape);
// get the static body that comes with the space
cpSpace *space = cpBodyGetSpace(body);
cpBody *static_body = cpSpaceGetStaticBody(space);
// ignore the static body
if (static_body == body)
return;
element *el = (element *) malloc(sizeof(element));
el->body = body;
element **l = (element **) data;
LL_APPEND(*l, el);
}
void DynamicObjectStabilizator::fixatePoints()
{
// vector<IDynamicObject *>::iterator begin = m_DynamicObjects.begin();
// vector<IDynamicObject *>::iterator end = m_DynamicObjects.end();
// vector<IDynamicObject *>::iterator iter = begin + 1;
// for( ; iter != end ; iter++ )
size_t count = m_DynamicObjects.size();
for( size_t object_i = 0 ; object_i < 2 ; object_i++ )
{
const IGeometryObject & geometryObjext = m_DynamicObjects[object_i]->getGeometryObject();
if( geometryObjext.getType() != GEOMETRYOBJECT_POINT )
{
// assert( false );
continue;
}
cpBody * kineticBody = cpBodyNewKinematic();
cpSpaceAddBody( m_Space, kineticBody );
m_KineticBodies.push_back( kineticBody );
const GeometryPoint & geometryPoint = dynamic_cast<const GeometryPoint &>( geometryObjext );
int x = geometryPoint.getX();
int y = geometryPoint.getY();
cpVect mousePoint = cpv( x, y );
cpShape * shape = cpSpacePointQueryNearest( m_Space, mousePoint, 100.0, GRAB_FILTER, 0 );
if( 0 == shape )
{
return;
}
cpVect new_mouse_position = cpv( x, y );
cpBodySetPosition( kineticBody, new_mouse_position );
cpBody * trackingBody = cpShapeGetBody( shape );
cpConstraint * joint = cpPivotJointNew2( kineticBody, trackingBody, cpvzero, cpvzero );
cpSpaceAddConstraint( m_Space, joint );
m_Joints.push_back( joint );
break; //one pointb
}
}
static cpSpaceDebugColor ColorForShape(cpShape *shape, cpDataPointer data)
{
if(cpShapeGetSensor(shape)){
return LAColor(1.0f, 0.3f);
} else {
cpBody *body = cpShapeGetBody(shape);
if(cpBodyIsSleeping(body)){
return LAColor(0.2f, 0.3f);
} else if(body->sleeping.idleTime > shape->space->sleepTimeThreshold) {
return LAColor(0.66f, 0.3f);
} else {
GLfloat intensity = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC ? 0.15f : 0.75f);
return RGBAColor(intensity, 0.0f, 0.0f, 0.3f);
}
}
}
void Slice::SliceShapePostStep(cpSpace *space, cpShape *shape, struct SliceContext *context)
{
cpVect a = context->a;
cpVect b = context->b;
// Clipping plane normal and distance.
cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
cpFloat dist = cpvdot(a, n);
ClipPoly(space, shape, n, dist);
ClipPoly(space, shape, cpvneg(n), -dist);
cpBody *body = cpShapeGetBody(shape);
cpSpaceRemoveShape(space, shape);
cpSpaceRemoveBody(space, body);
cpShapeFree(shape);
cpBodyFree(body);
}
void Slice::ClipPoly(cpSpace *space, cpShape *shape, cpVect n, cpFloat dist)
{
cpBody *body = cpShapeGetBody(shape);
int count = cpPolyShapeGetCount(shape);
int clippedCount = 0;
cpVect *clipped = (cpVect *)alloca((count + 1)*sizeof(cpVect));
for(int i=0, j=count-1; i<count; j=i, i++){
cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(shape, j));
cpFloat a_dist = cpvdot(a, n) - dist;
if(a_dist < 0.0){
clipped[clippedCount] = a;
clippedCount++;
}
cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(shape, i));
cpFloat b_dist = cpvdot(b, n) - dist;
if(a_dist*b_dist < 0.0f){
cpFloat t = cpfabs(a_dist)/(cpfabs(a_dist) + cpfabs(b_dist));
clipped[clippedCount] = cpvlerp(a, b, t);
clippedCount++;
}
}
cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
cpFloat mass = cpAreaForPoly(clippedCount, clipped, 0.0f)*DENSITY;
cpFloat moment = cpMomentForPoly(mass, clippedCount, clipped, cpvneg(centroid), 0.0f);
cpBody *new_body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpBodySetPosition(new_body, centroid);
cpBodySetVelocity(new_body, cpBodyGetVelocityAtWorldPoint(body, centroid));
cpBodySetAngularVelocity(new_body, cpBodyGetAngularVelocity(body));
cpTransform transform = cpTransformTranslate(cpvneg(centroid));
cpShape *new_shape = cpSpaceAddShape(space, cpPolyShapeNew(new_body, clippedCount, clipped, transform, 0.0));
// Copy whatever properties you have set on the original shape that are important
cpShapeSetFriction(new_shape, cpShapeGetFriction(shape));
}
static void
update(cpSpace *space)
{
cpFloat tolerance = 2.0;
if(ChipmunkDemoRightClick && cpShapeNearestPointQuery(shape, ChipmunkDemoMouse, NULL) > tolerance){
cpBody *body = cpShapeGetBody(shape);
int count = cpPolyShapeGetNumVerts(shape);
// Allocate the space for the new vertexes on the stack.
cpVect *verts = (cpVect *)alloca((count + 1)*sizeof(cpVect));
for(int i=0; i<count; i++){
verts[i] = cpPolyShapeGetVert(shape, i);
}
verts[count] = cpBodyWorld2Local(body, ChipmunkDemoMouse);
// This function builds a convex hull for the vertexes.
// Because the result array is NULL, it will reduce the input array instead.
int hullCount = cpConvexHull(count + 1, verts, NULL, NULL, tolerance);
// Figure out how much to shift the body by.
cpVect centroid = cpCentroidForPoly(hullCount, verts);
// Recalculate the body properties to match the updated shape.
cpFloat mass = cpAreaForPoly(hullCount, verts)*DENSITY;
cpBodySetMass(body, mass);
cpBodySetMoment(body, cpMomentForPoly(mass, hullCount, verts, cpvneg(centroid)));
cpBodySetPos(body, cpBodyLocal2World(body, centroid));
// Use the setter function from chipmunk_unsafe.h.
// You could also remove and recreate the shape if you wanted.
cpPolyShapeSetVerts(shape, hullCount, verts, cpvneg(centroid));
}
int steps = 1;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
}
}
static void
update(cpSpace *space, double dt)
{
if(ChipmunkDemoRightDown){
cpShape *nearest = cpSpaceNearestPointQueryNearest(space, ChipmunkDemoMouse, 0.0, GRABABLE_MASK_BIT, CP_NO_GROUP, NULL);
if(nearest){
cpBody *body = cpShapeGetBody(nearest);
if(cpBodyIsStatic(body)){
cpSpaceConvertBodyToDynamic(space, body, pentagon_mass, pentagon_moment);
cpSpaceAddBody(space, body);
} else {
cpSpaceRemoveBody(space, body);
cpSpaceConvertBodyToStatic(space, body);
}
}
}
cpSpaceStep(space, dt);
cpSpaceEachBody(space, &eachBody, NULL);
}
static void DrawShape(cpShape *shape, DrawNode *renderer)
{
cpBody *body = cpShapeGetBody(shape);
Color4F color = ColorForBody(body);
switch (shape->CP_PRIVATE(klass)->type)
{
case CP_CIRCLE_SHAPE:
{
cpCircleShape *circle = (cpCircleShape *)shape;
cpVect center = circle->tc;
cpFloat radius = circle->r;
renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color);
renderer->drawSegment(cpVert2Point(center), cpVert2Point(cpvadd(center, cpvmult(cpBodyGetRotation(body), radius))), 1.0, color);
}
break;
case CP_SEGMENT_SHAPE:
{
cpSegmentShape *seg = (cpSegmentShape *)shape;
renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 2.0), color);
}
break;
case CP_POLY_SHAPE:
{
cpPolyShape* poly = (cpPolyShape*)shape;
Color4F line = color;
line.a = cpflerp(color.a, 1.0, 0.5);
int num = poly->count;
Vec2* pPoints = new (std::nothrow) Vec2[num];
for(int i=0;i<num;++i)
pPoints[i] = cpVert2Point(poly->planes[i].v0);
renderer->drawPolygon(pPoints, num, color, 1.0, line);
CC_SAFE_DELETE_ARRAY(pPoints);
}
break;
default:
cpAssertHard(false, "Bad assertion in DrawShape()");
}
}
// adds an impulse shape to the cpSpace
cpBody * core_add_new_shape_with_impulse ( cpSpace *space, const int type, const double p1x, const double p1y, const double p2x, const double p2y, Color *color, const double orientation, const double friction, const double elasticity, const double density, const int index, cpVect impulse, cpVect offset ) {
cpShape * shape;
if ( type == BOX_TYPE || type == CIRCLE_TYPE ) {
shape = core_add_new_shape ( space, type, p1x, p1y, p2x, p2y, color, orientation, friction, elasticity, density, index );
} else {
shape = core_add_single_segment_shape ( space, p1x, p1y, p2x, p2y, color, friction, elasticity, density, index );
} if ( shape == NULL ) return NULL;
DrawShapeInfo *info = cpShapeGetUserData ( shape );
info->space_shape_type = SPACE_TYPE_PLANET_R;
cpBody *body = cpShapeGetBody ( shape );
impulse = cpv ( impulse.x * IMPULSE_MULTIPLIER, impulse.y * IMPULSE_MULTIPLIER );
cpBodyApplyImpulse ( body, impulse, offset );
return body;
}
void planeCloud(cpShape * planeShape, int num) {
Player *p1 = (Player*)cpBodyGetUserData(cpShapeGetBody(planeShape));
p1->setInCloud(num);
}
cpBody *shape::body( void )
{
return cpShapeGetBody( this->m_shape );
}
cpBool Buoyancy::WaterPreSolve(cpArbiter *arb, cpSpace *space, void *ptr)
{
CP_ARBITER_GET_SHAPES(arb, water, poly);
cpBody *body = cpShapeGetBody(poly);
// Get the top of the water sensor bounding box to use as the water level.
cpFloat level = cpShapeGetBB(water).t;
// Clip the polygon against the water level
int count = cpPolyShapeGetCount(poly);
int clippedCount = 0;
#ifdef _MSC_VER
// MSVC is pretty much the only compiler in existence that doesn't support variable sized arrays.
cpVect clipped[10];
#else
cpVect clipped[count + 1];
#endif
for(int i=0, j=count-1; i<count; j=i, i++){
cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, j));
cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, i));
if(a.y < level){
clipped[clippedCount] = a;
clippedCount++;
}
cpFloat a_level = a.y - level;
cpFloat b_level = b.y - level;
if(a_level*b_level < 0.0f){
cpFloat t = cpfabs(a_level)/(cpfabs(a_level) + cpfabs(b_level));
clipped[clippedCount] = cpvlerp(a, b, t);
clippedCount++;
}
}
// Calculate buoyancy from the clipped polygon area
cpFloat clippedArea = cpAreaForPoly(clippedCount, clipped, 0.0f);
cpFloat displacedMass = clippedArea*FLUID_DENSITY;
cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
cpDataPointer data = ptr;
DrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f), data);
DrawDot(5, centroid, RGBAColor(0, 0, 1, 1), data);
cpFloat dt = cpSpaceGetCurrentTimeStep(space);
cpVect g = cpSpaceGetGravity(space);
// Apply the buoyancy force as an impulse.
cpBodyApplyImpulseAtWorldPoint(body, cpvmult(g, -displacedMass*dt), centroid);
// Apply linear damping for the fluid drag.
cpVect v_centroid = cpBodyGetVelocityAtWorldPoint(body, centroid);
cpFloat k = k_scalar_body(body, centroid, cpvnormalize(v_centroid));
cpFloat damping = clippedArea*FLUID_DRAG*FLUID_DENSITY;
cpFloat v_coef = cpfexp(-damping*dt*k); // linear drag
// cpFloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag
cpBodyApplyImpulseAtWorldPoint(body, cpvmult(cpvsub(cpvmult(v_centroid, v_coef), v_centroid), 1.0/k), centroid);
// Apply angular damping for the fluid drag.
cpVect cog = cpBodyLocalToWorld(body, cpBodyGetCenterOfGravity(body));
cpFloat w_damping = cpMomentForPoly(FLUID_DRAG*FLUID_DENSITY*clippedArea, clippedCount, clipped, cpvneg(cog), 0.0f);
cpBodySetAngularVelocity(body, cpBodyGetAngularVelocity(body)*cpfexp(-w_damping*dt/cpBodyGetMoment(body)));
return cpTrue;
}
