static void DrawConstraint(cpConstraint *constraint, DrawNode *renderer)
{
cpBody *body_a = constraint->a;
cpBody *body_b = constraint->b;
const cpConstraintClass *klass = constraint->CP_PRIVATE(klass);
if (klass == cpPinJointGetClass())
{
cpPinJoint *joint = (cpPinJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
cpVect b = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
}
else if (klass == cpSlideJointGetClass())
{
cpSlideJoint *joint = (cpSlideJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
cpVect b = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
}
else if (klass == cpPivotJointGetClass())
{
cpPivotJoint *joint = (cpPivotJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
cpVect b = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
}
else if (klass == cpGrooveJointGetClass())
{
cpGrooveJoint *joint = (cpGrooveJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->grv_a);
cpVect b = cpBodyLocal2World(body_a, joint->grv_b);
cpVect c = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2Point(c), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
}
else if (klass == cpDampedSpringGetClass())
{
// TODO
}
else
{
// printf("Cannot draw constraint\n");
}
}
cpDampedSpring *
cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
{
cpConstraintInit((cpConstraint *)spring, cpDampedSpringGetClass(), a, b);
spring->anchr1 = anchr1;
spring->anchr2 = anchr2;
spring->restLength = restLength;
spring->stiffness = stiffness;
spring->damping = damping;
spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce;
return spring;
}
const cpConstraintClass *DampedSpring::getClass()
{
return cpDampedSpringGetClass();
}
void PhysicsDebugDraw::drawJoint(PhysicsJoint& joint)
{
for (auto it = joint._info->getJoints().begin(); it != joint._info->getJoints().end(); ++it)
{
cpConstraint *constraint = *it;
cpBody *body_a = constraint->a;
cpBody *body_b = constraint->b;
const cpConstraintClass *klass = constraint->klass_private;
if(klass == cpPinJointGetClass())
{
cpPinJoint *subJoint = (cpPinJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpSlideJointGetClass())
{
cpSlideJoint *subJoint = (cpSlideJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpPivotJointGetClass())
{
cpPivotJoint *subJoint = (cpPivotJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpGrooveJointGetClass())
{
cpGrooveJoint *subJoint = (cpGrooveJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->grv_a, body_a->rot));
cpVect b = cpvadd(body_a->p, cpvrotate(subJoint->grv_b, body_a->rot));
cpVect c = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(c), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpDampedSpringGetClass())
{
cpDampedSpring *subJoint = (cpDampedSpring *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
}
}
TiXmlElement *cpSpaceSerializer::createConstraintElm(cpConstraint* constraint)
{
TiXmlElement *elm;
CPSS_ID id, body_a_id, body_b_id;
if (delegate)
{
id = delegate->makeId(constraint);
delegate->writing(constraint, id);
body_a_id = delegate->makeId(constraint->a);
body_b_id = delegate->makeId(constraint->b);
}
else
{
id = CPSS_DEFAULT_MAKE_ID(constraint);
body_a_id = CPSS_DEFAULT_MAKE_ID(constraint->a);
body_b_id = CPSS_DEFAULT_MAKE_ID(constraint->b);
}
//Correct id's for staticBody
if (constraint->a == _space->staticBody)
body_a_id = 0;
if (constraint->b == _space->staticBody)
body_b_id = 0;
//Specific
const cpConstraintClass *klass = constraint->CP_PRIVATE(klass);
if(klass == cpPinJointGetClass())
elm = createPinJointElm((cpPinJoint*)constraint);
else if(klass == cpSlideJointGetClass())
elm = createSlideJointElm((cpSlideJoint*)constraint);
else if(klass == cpPivotJointGetClass())
elm = createPivotJointElm((cpPivotJoint*)constraint);
else if(klass == cpGrooveJointGetClass())
elm = createGrooveJointElm((cpGrooveJoint*)constraint);
else if (klass == cpSimpleMotorGetClass())
elm = createMotorJointElm((cpSimpleMotor*)constraint);
else if (klass == cpGearJointGetClass())
elm = createGearJointElm((cpGearJoint*)constraint);
else if(klass == cpDampedSpringGetClass())
elm = createSpringJointElm((cpDampedSpring*)constraint);
else if(klass == cpRotaryLimitJointGetClass())
elm = createRotaryLimitJointElm((cpRotaryLimitJoint*)constraint);
else if(klass == cpRatchetJointGetClass())
elm = createRatchetJointElm((cpRatchetJoint*)constraint);
else if(klass == cpDampedRotarySpringGetClass())
elm = createRotarySpringJointElm((cpDampedRotarySpring*)constraint);
else
elm = new TiXmlElement("unknown");
//Generic
elm->LinkEndChild(createValueElm("id", id));
elm->LinkEndChild(createValueElm("body_a_id", body_a_id));
elm->LinkEndChild(createValueElm("body_b_id", body_b_id));
elm->LinkEndChild(createValueElm("maxForce", constraint->maxForce));
elm->LinkEndChild(createValueElm("errorBias", constraint->errorBias));
elm->LinkEndChild(createValueElm("maxBias", constraint->maxBias));
return elm;
}
