void Mirror::SetRotationAngle(float a_Angle)
{
//rotate physbodies
cpBodySetAngle(m_pBody, cpFloat(a_Angle));
cpBodySetAngle(m_pReflectBody, cpFloat(a_Angle));
//rotate sprite
m_Sprite.sprite->setRotation( (a_Angle * 180) / 3.14159265f );
}
bool CDynamics2DBoxEntity::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_check_only) {
SInt32 nCollision;
/* Check whether the box is movable or not */
if(m_cBoxEntity.GetEmbodiedEntity().IsMovable()) {
/* The box is movable */
/* Save body position and orientation */
cpVect tOldPos = m_ptBody->p;
cpFloat fOldA = m_ptBody->a;
/* Move the body to the desired position */
m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create a shape sensor to test the movement */
/* First construct the vertices */
CVector3 cHalfSize = m_cBoxEntity.GetSize() * 0.5f;
cpVect tVertices[] = {
cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
cpv(-cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), -cHalfSize.GetY())
};
/* Then create the shape itself */
cpShape* ptTestShape = cpPolyShapeNew(m_ptBody,
4,
tVertices,
cpvzero);
/* Check if there is a collision */
nCollision = cpSpaceShapeQuery(m_cEngine.GetPhysicsSpace(), ptTestShape, NULL, NULL);
/* Dispose of the sensor shape */
cpShapeFree(ptTestShape);
if(b_check_only || nCollision) {
/* Restore old body state if there was a collision or
it was only a check for movement */
m_ptBody->p = tOldPos;
cpBodySetAngle(m_ptBody, fOldA);
}
else {
/* Update the active space hash if the movement is actual */
cpSpaceReindexShape(m_cEngine.GetPhysicsSpace(), m_ptShape);
}
}
else {
/* The box is not movable, so you can't move it :-) */
nCollision = 1;
}
/* The movement is allowed if there is no collision */
return !nCollision;
}
void RCPBody::setAngle(float a)
{
mAngle = a;
if (mBody) {
cpBodySetAngle(mBody, degToRad(mAngle));
}
}
void PhysicsBody::setRotation(float rotation)
{
if (!_rotationResetTag)
{
cpBodySetAngle(_info->getBody(), -PhysicsHelper::float2cpfloat((rotation + _rotationOffset) * (M_PI / 180.0f)));
}
}
cpBody*
cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
{
body->velocity_func = cpBodyUpdateVelocityDefault;
body->position_func = cpBodyUpdatePositionDefault;
cpBodySetMass(body, m);
cpBodySetMoment(body, i);
body->p = cpvzero;
body->v = cpvzero;
body->f = cpvzero;
cpBodySetAngle(body, 0.0f);
body->w = 0.0f;
body->t = 0.0f;
body->v_bias = cpvzero;
body->w_bias = 0.0f;
body->data = NULL;
body->v_limit = (cpFloat)INFINITY;
body->w_limit = (cpFloat)INFINITY;
body->space = NULL;
body->shapesList = NULL;
cpComponentNode node = {NULL, NULL, 0, 0.0f};
body->node = node;
return body;
}
ETERM *body_copy(ETERM *fromp, ETERM *argp) {
// get the args
ETERM *space_refp = erl_element(1, argp);
ETERM *idp = erl_element(2, argp);
ETERM *from_idp = erl_element(3, argp);
erlmunk_space *s;
int space_id = ERL_REF_NUMBER(space_refp);
HASH_FIND_INT(erlmunk_spaces, &space_id, s);
int body_id = ERL_INT_VALUE(idp);
erlmunk_body *b;
HASH_FIND_INT(s->bodies, &body_id, b);
if (b == NULL)
return NULL;
int from_body_id = ERL_INT_VALUE(from_idp);
erlmunk_body *from_b;
HASH_FIND_INT(s->bodies, &from_body_id, from_b);
// DEBUGF(("copying location from body #%d(%p) to #%d(%p)",
// from_body_id, from_b, body_id, b));
// copy position and angle from the from body
cpBodySetPosition(b->body, cpBodyGetPosition(from_b->body));
cpBodySetAngle(b->body, cpBodyGetAngle(from_b->body));
cpBodySetVelocity(b->body, cpBodyGetVelocity(from_b->body));
return NULL;
}
cpBody*
cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
{
body->velocity_func = cpBodyUpdateVelocity;
body->position_func = cpBodyUpdatePosition;
cpBodySetMass(body, m);
cpBodySetMoment(body, i);
body->p = cpvzero;
body->v = cpvzero;
body->f = cpvzero;
cpBodySetAngle(body, 0.0f);
body->w = 0.0f;
body->t = 0.0f;
body->v_bias = cpvzero;
body->w_bias = 0.0f;
body->data = NULL;
// body->active = 1;
return body;
}
Shell(cpVect pos, cpVect vel, float angle)
{
cpVect vl[4] = {cpv(0, 0), cpv(0.1, 0), cpv(0.07, 0.3), cpv(0.03, 0.3)};
int vn = sizeof(vl)/sizeof(cpVect);
float mass = cpAreaForPoly(vn, vl, 0) * shell_density;
float moi = cpMomentForPoly(mass, vn, vl, cpv(0, 0), 0);
body = cpBodyNew(mass, moi);
cpshape = cpPolyShapeNew(body, vn, vl, cpTransformIdentity, 0);
cpShapeSetFriction(cpshape, 0.9);
cpVect centroid = cpCentroidForPoly(vn, vl);
shape.setPointCount(vn);
for (int i = 0; i < vn; i++)
{
shape.setPoint(i, sf::Vector2f(vl[i].x, vl[i].y));
}
cpBodySetCenterOfGravity(body, centroid);
cpBodySetPosition(body, pos-centroid);
cpBodySetVelocity(body, vel);
cpBodySetAngle(body, angle);
cpShapeSetCollisionType(cpshape, 2);
cpShapeSetUserData(cpshape, this);
}
static void
add_box(cpSpace *space)
{
const cpFloat size = 10.0f;
const cpFloat mass = 1.0f;
cpVect verts[] = {
cpv(-size,-size),
cpv(-size, size),
cpv( size, size),
cpv( size,-size),
};
cpFloat radius = cpvlength(cpv(size, size));
cpVect pos = rand_pos(radius);
cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForPoly(mass, 4, verts, cpvzero, 0.0f)));
body->velocity_func = planetGravityVelocityFunc;
cpBodySetPosition(body, pos);
// Set the box's velocity to put it into a circular orbit from its
// starting position.
cpFloat r = cpvlength(pos);
cpFloat v = cpfsqrt(gravityStrength / r) / r;
cpBodySetVelocity(body, cpvmult(cpvperp(pos), v));
// Set the box's angular velocity to match its orbital period and
// align its initial angle with its position.
cpBodySetAngularVelocity(body, v);
cpBodySetAngle(body, cpfatan2(pos.y, pos.x));
cpShape *shape = cpSpaceAddShape(space, cpPolyShapeNew(body, 4, verts, cpTransformIdentity, 0.0));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.7f);
}
void CDynamics2DMultiBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Set target position and orientation */
cpVect tBodyPos = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpFloat tBodyOrient = cZAngle.GetValue();
/* For each body: */
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
/* Set body orientation at anchor */
cpBodySetAngle(m_vecBodies[i].Body,
tBodyOrient + m_vecBodies[i].OffsetOrient);
/* Set body position at anchor */
cpBodySetPos(m_vecBodies[i].Body,
cpvadd(tBodyPos,
cpvrotate(m_vecBodies[i].OffsetPos,
m_vecBodies[i].Body->rot)));
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(),
m_vecBodies[i].Body);
}
/* Update ARGoS entity state */
UpdateEntityStatus();
}
void physics_add(Entity ent)
{
PhysicsInfo *info;
if (entitypool_get(pool, ent))
return; /* already has physics */
transform_add(ent);
info = entitypool_add(pool, ent);
info->mass = 1.0;
info->type = PB_DYNAMIC;
/* create, init cpBody */
info->body = cpSpaceAddBody(space, cpBodyNew(info->mass, 1.0));
cpBodySetUserData(info->body, ent); /* for cpBody -> Entity mapping */
cpBodySetPos(info->body, cpv_of_vec2(transform_get_position(ent)));
cpBodySetAngle(info->body, transform_get_rotation(ent));
info->last_dirty_count = transform_get_dirty_count(ent);
/* initially no shapes */
info->shapes = array_new(ShapeInfo);
/* initialize last_pos/last_ang info for kinematic bodies */
info->last_pos = cpBodyGetPos(info->body);
info->last_ang = cpBodyGetAngle(info->body);
info->collisions = NULL;
}
static void _update_kinematics()
{
PhysicsInfo *info;
cpVect pos;
cpFloat ang;
Scalar invdt;
Entity ent;
if (timing_dt <= FLT_EPSILON)
return;
invdt = 1 / timing_dt;
entitypool_foreach(info, pool)
if (info->type == PB_KINEMATIC)
{
ent = info->pool_elem.ent;
/* move to transform */
pos = cpv_of_vec2(transform_get_position(ent));
ang = transform_get_rotation(ent);
cpBodySetPos(info->body, pos);
cpBodySetAngle(info->body, ang);
info->last_dirty_count = transform_get_dirty_count(ent);
/* update linear, angular velocities based on delta */
cpBodySetVel(info->body,
cpvmult(cpvsub(pos, info->last_pos), invdt));
cpBodySetAngVel(info->body, (ang - info->last_ang) * invdt);
cpSpaceReindexShapesForBody(space, info->body);
/* save current state for next computation */
info->last_pos = pos;
info->last_ang = ang;
}
}
static void grabObject(cpBody *body) {
cpVect lp;
lp = cpBodyWorld2Local(body, atlMouseClickPos());
atlCreateMouseJoint(g_Space, body, lp);
cpBodySetAngle(body, M_PI/180);
}
void NaiveRotationAlgorithm::stepSystem(SystemInfo *individual)
{
MachineSystem *oldSystem = individual->system;
individual->system = new MachineSystem(*oldSystem); // copy it to stop all the damn bouncing about
delete oldSystem;
cpBody *inputBody = individual->system->partAtPosition(individual->system->inputMachinePosition)->body;
cpBody *outputBody = individual->system->partAtPosition(individual->system->outputMachinePosition)->body;
cpSpace *systemSpace = individual->system->getSpace();
cpConstraint *motor = cpSimpleMotorNew(cpSpaceGetStaticBody(systemSpace), inputBody, M_PI);
cpSpaceAddConstraint(systemSpace, motor);
cpConstraintSetMaxForce(motor, 50000);
for (int i=0; i<simSteps; i++) {
individual->inputValues[i] = (cpBodyGetAngle(inputBody));
cpSpaceStep(systemSpace, 0.1);
individual->outputValues[i] = (cpBodyGetAngle(outputBody));
}
cpSpaceRemoveConstraint(systemSpace, motor);
cpBodySetAngVel(outputBody, 0);
cpBodySetAngle(inputBody, 0);
}
__declspec( dllexport ) void push( const void * _in, int in_size, void * _out, int out_sz )
{
int index;
Variable *var;
cpBody *body;
cpShape *shape;
index = PEEKINT(INPUT_MEMBLOCK,0);
var = vhGetVariable(&mVariableHandler,index);
switch (var->mType)
{
case VarTypeBody:
body = (cpBody*)var->mPtr;
cpBodySetAngle(body,degToRad(PEEKFLOAT(INPUT_MEMBLOCK,4)));
cpBodySetPos(body,PEEKVECT(INPUT_MEMBLOCK,8));
cpBodySetAngVel(body,degToRad(PEEKFLOAT(INPUT_MEMBLOCK,16)));
cpBodySetVel(body,PEEKVECT(INPUT_MEMBLOCK,20));
break;
case VarTypeShape:
shape = (cpShape*)var->mPtr;
cpShapeSetFriction(shape,PEEKFLOAT(INPUT_MEMBLOCK,4));
cpShapeSetElasticity(shape,PEEKFLOAT(INPUT_MEMBLOCK,8));
cpShapeSetLayers(shape,PEEKUINT(INPUT_MEMBLOCK,12));
cpShapeSetGroup(shape,PEEKUINT(INPUT_MEMBLOCK,16));
break;
}
}
int lc_body_newindex(lua_State *vm){
//userdata, key, data
const char *key = lua_tostring(vm, 2);
cpBody *body = (lc_GetBody(1, vm))->body;
if (strcmp("pos", key) == 0 && lua_istable(vm, 3)){
cpBodySetPos(body, lc_TableTocpVect(3, vm));
}
else if (strcmp("vel", key) == 0 && lua_istable(vm, 3)){
cpBodySetVel(body, lc_TableTocpVect(3, vm));
}
else if (strcmp("angle", key) == 0 || strcmp("radangle", key) == 0){
cpBodySetAngle(body, (cpFloat)lua_tonumber(vm, 3));
}
else if (strcmp("degangle", key) == 0){
cpBodySetAngle(body, ( (cpFloat)lua_tonumber(vm, 3)*(cpFloat)M_PI/(cpFloat)180.f ) );
}
return 0;
}
void landerRotate(int direction, Lander l) //-1 counterclockwise, 1 clockwise
{
if(abs(l->orientation + direction) >= MAX_ORIENTATION)
return;
l->orientation += direction;
cpBodySetAngle(l->body, radiansFromOrientation(l->orientation));
}
void
cpBodyUpdatePosition(cpBody *body, cpFloat dt)
{
body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt);
body->v_bias = cpvzero;
body->w_bias = 0.0f;
}
void LinkerAgent::UpdateFromEngine()
{
DJVector2 djPosition = m_pSprite->GetPosition();
float djAngle = m_pSprite->GetRotation();
cpVect cpPosition;
cpPosition.x = (float) djPosition.x();
cpPosition.y = (float) -djPosition.y();
cpBodySetAngle(m_pBody, djAngle);
cpBodySetPos(m_pBody, cpPosition);
}
void RigidBody2D::CopyBodyData(cpBody* from, cpBody* to)
{
cpBodySetAngle(to, cpBodyGetAngle(from));
cpBodySetAngularVelocity(to, cpBodyGetAngularVelocity(from));
cpBodySetCenterOfGravity(to, cpBodyGetCenterOfGravity(from));
cpBodySetForce(to, cpBodyGetForce(from));
cpBodySetPosition(to, cpBodyGetPosition(from));
cpBodySetTorque(to, cpBodyGetTorque(from));
cpBodySetVelocity(to, cpBodyGetVelocity(from));
}
void RigidBody2D::CopyBodyData(cpBody* body)
{
cpBodySetAngle(m_handle, cpBodyGetAngle(body));
cpBodySetAngularVelocity(m_handle, cpBodyGetAngularVelocity(body));
cpBodySetCenterOfGravity(m_handle, cpBodyGetCenterOfGravity(body));
cpBodySetForce(m_handle, cpBodyGetForce(body));
cpBodySetPosition(m_handle, cpBodyGetPosition(body));
cpBodySetTorque(m_handle, cpBodyGetTorque(body));
cpBodySetVelocity(m_handle, cpBodyGetVelocity(body));
}
void CCPhysicsSprite::setRotation(float fRotation)
{
if (m_bIgnoreBodyRotation)
{
CCSprite::setRotation(fRotation);
}
else
{
cpBodySetAngle(m_pCPBody, -CC_DEGREES_TO_RADIANS(fRotation));
}
}
void RigidBody2D::SetRotation(float rotation)
{
cpBodySetAngle(m_handle, ToRadians(rotation));
if (m_isStatic)
{
m_world->RegisterPostStep(this, [](Nz::RigidBody2D* body)
{
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
});
}
}
static int l_physics_setBodyAngle(lua_State* state)
{
l_tools_checkUserDataPlusErrMsg(state, 1, "You must provide a body");
l_physics_Body* body = (l_physics_Body*)lua_touserdata(state, 1);
float value = l_tools_toNumberOrError(state, 2);
cpBodySetAngle(body->body, value);
return 0;
}
void KRSimulator2D::step(double time)
{
mCollisions.clear();
cpSpaceStep((cpSpace*)mCPSpace, time);
if (mHasChangedAngle) {
cpBodySetAngle((cpBody*)mCPStaticBody, mNextAngle);
cpSpaceRehashStatic((cpSpace*)mCPSpace);
mHasChangedAngle = false;
}
}
void CDynamics2DSingleBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Move the body to the desired position */
m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
/* Update ARGoS entity state */
UpdateEntityStatus();
}
// adds nonstatic box shape to space
// currently does not roate box
static cpShape *core_add_box_shape ( cpSpace *space, Box *box, const int index ) {
// calculate mass and moment of a box
cpFloat mass = box->density * box->width * box->height;
cpFloat moment = cpMomentForBox ( mass, box->width, box->height );
// add body with mass and moment of a square to space
cpBody *body = cpBodyNew ( mass, moment );
cpSpaceAddPostStepCallback ( space, (cpPostStepFunc) postStepAddBody, body, NULL );
cpBodySetPos ( body, cpv ( box->x, box->y ) );
// set index of body
BodyInfo * bi = body_info_new(0);
bi->index = index;
bi->type = BOX_TYPE;
bi->p1x = box->x - (box->width) / 2.0;
bi->p1y = box->y + (box->height) / 2.0;
bi->p2x = box->x + (box->width) / 2.0;;
bi->p2y = box->y - (box->height) / 2.0;
bi->color->r = box->color->r;
bi->color->g = box->color->g;
bi->color->b = box->color->b;
bi->friction = box->friction;
bi->density = box->density;
bi->elasticity = box->elasticity;
body->data = bi;
double hw = ( box->width ) / 2.0;
double hh = ( box->height ) / 2.0;
cpVect cpv1 = cpv ( -hw,-hh );
cpVect cpv2 = cpv ( -hw, hh );
cpVect cpv3 = cpv ( hw, hh );
cpVect cpv4 = cpv ( hw, -hh );
cpVect verts [4]= { cpv1, cpv2, cpv3, cpv4 };
// add box collision shape to body
cpShape *boxShape = cpPolyShapeNew ( body, 4, verts, cpv ( 0, 0 ) );
cpSpaceAddPostStepCallback ( space, (cpPostStepFunc) postStepAddShape, boxShape, NULL );
cpShapeSetFriction ( boxShape, box->friction );
cpShapeSetElasticity ( boxShape, box->elasticity );
cpBodySetAngle ( body, box->angle);
DrawShapeInfo *info = add_box_draw_shape_info ( box );
boxShape->data= ( cpDataPointer ) info;
return boxShape;
}
cpBody *cpSpaceSerializer::createBody(TiXmlElement *elm)
{
TiXmlElement *bodyElm;;
cpBody *body;
CPSS_ID b_id = createValue<CPSS_ID>("body_id", elm);
BodyMap::iterator itr = _bodyMap.find(b_id);
//If it doesn't exist, try to create it
if (itr == _bodyMap.end())
{
bodyElm = elm->FirstChildElement("body");
if (bodyElm)
{
cpFloat mass = createValue<cpFloat>("mass", bodyElm);
cpFloat inertia = createValue<cpFloat>("inertia", bodyElm);
if (mass == INFINITY)
body = cpBodyNewStatic();
else
body = cpBodyNew(mass, inertia);
body->p = createPoint("p", bodyElm);
body->v = createPoint("v", bodyElm);
body->f = createPoint("f", bodyElm);
body->w = createValue<cpFloat>("w", bodyElm);
body->t = createValue<cpFloat>("t", bodyElm);
cpBodySetAngle(body, createValue<cpFloat>("a", bodyElm));
_bodyMap[b_id] = body;
if (delegate && b_id != 0)
{
if (!delegate->reading(body, b_id))
{
cpBodyFree(body);
body = NULL;
}
}
}
else
body = cpBodyNewStatic(); //Fail case, should throw or something
}
else
body = itr->second; //Else grab it
return body;
}
void CDynamics2DCylinderEntity::Reset() {
if(m_ptBody != NULL) {
/* Reset body position */
const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
/* Reset body orientation */
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Zero speed and applied forces */
m_ptBody->v = cpvzero;
m_ptBody->w = 0.0f;
cpBodyResetForces(m_ptBody);
}
}
void PhysicsSprite::setRotation(float fRotation)
{
if (_ignoreBodyRotation)
{
Sprite::setRotation(fRotation);
}
#if CC_ENABLE_CHIPMUNK_INTEGRATION
else
{
cpBodySetAngle(_CPBody, -CC_DEGREES_TO_RADIANS(fRotation));
}
#elif CC_ENABLE_BOX2D_INTEGRATION
else
{
