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; }