btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0),m_limit() { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; }
void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) { // convert target from body to constraint space btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); qConstraint.normalize(); // extract "pure" hinge component btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); btQuaternion qHinge = qNoHinge.inverse() * qConstraint; qHinge.normalize(); // compute angular target, clamped to limits btScalar targetAngle = qHinge.getAngle(); if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. { qHinge = -(qHinge); targetAngle = qHinge.getAngle(); } if (qHinge.getZ() < 0) targetAngle = -targetAngle; setMotorTarget(targetAngle, dt); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), #ifdef _BT_USE_CENTER_LIMIT_ m_limit(), #endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0), m_normalCFM(0), m_normalERP(0), m_stopCFM(0), m_stopERP(0) { m_rbAFrame.getOrigin() = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - SIMD_EPSILON) { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else if (projection <= -1.0f + SIMD_EPSILON) { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); #ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
/** Initialises an item. * \param type Type for this item. * \param xyz The position for this item. * \param normal The normal for this item. */ void ItemState::initItem(ItemType type, const Vec3& xyz, const Vec3& normal) { m_xyz = xyz; m_original_rotation = shortestArcQuat(Vec3(0, 1, 0), normal); m_original_type = ITEM_NONE; m_ticks_till_return = 0; setDisappearCounter(); } // initItem
Item::Item(ItemType type, const Vec3& xyz, const Vec3& normal, scene::IMesh* mesh, scene::IMesh* lowres_mesh) { assert(type != ITEM_TRIGGER); // use other constructor for that m_distance_2 = 1.2f; initItem(type, xyz); m_original_rotation = shortestArcQuat(Vec3(0, 1, 0), normal); m_rotation_angle = 0.0f; m_original_mesh = mesh; m_original_lowmesh = lowres_mesh; m_listener = NULL; LODNode* lodnode = new LODNode("item", irr_driver->getSceneManager()->getRootSceneNode(), irr_driver->getSceneManager()); scene::IMeshSceneNode* meshnode = irr_driver->addMesh(mesh, StringUtils::insertValues("item_%i", (int)type)); if (lowres_mesh != NULL) { lodnode->add(35, meshnode, true); scene::IMeshSceneNode* meshnode = irr_driver->addMesh(lowres_mesh, StringUtils::insertValues("item_lo_%i", (int)type)); lodnode->add(100, meshnode, true); } else { lodnode->add(100, meshnode, true); } m_node = lodnode; //m_node = irr_driver->addMesh(mesh); #ifdef DEBUG std::string debug_name("item: "); debug_name += m_type; m_node->setName(debug_name.c_str()); #endif World::getWorld()->getTrack()->adjustForFog(m_node); m_node->setAutomaticCulling(scene::EAC_FRUSTUM_BOX); m_node->setPosition(xyz.toIrrVector()); Vec3 hpr; hpr.setHPR(m_original_rotation); m_node->setRotation(hpr.toIrrHPR()); m_node->grab(); } // Item(type, xyz, normal, mesh, lowres_mesh)
void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) { m_qTarget = q; // clamp motor target to within limits { btScalar softness = 1.f;//m_limitSoftness; // split into twist and cone btVector3 vTwisted = quatRotate(m_qTarget, vTwist); btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); // clamp cone if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) { btScalar swingAngle, swingLimit; btVector3 swingAxis; computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); if (fabs(swingAngle) > SIMD_EPSILON) { if (swingAngle > swingLimit*softness) swingAngle = swingLimit*softness; else if (swingAngle < -swingLimit*softness) swingAngle = -swingLimit*softness; qTargetCone = btQuaternion(swingAxis, swingAngle); } } // clamp twist if (m_twistSpan >= btScalar(0.05f)) { btScalar twistAngle; btVector3 twistAxis; computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); if (fabs(twistAngle) > SIMD_EPSILON) { // eddy todo: limitSoftness used here??? if (twistAngle > m_twistSpan*softness) twistAngle = m_twistSpan*softness; else if (twistAngle < -m_twistSpan*softness) twistAngle = -m_twistSpan*softness; qTargetTwist = btQuaternion(twistAxis, twistAngle); } } m_qTarget = qTargetCone * qTargetTwist; } }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), m_enableAngularMotor(false) { m_rbAFrame.getOrigin() = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - SIMD_EPSILON) { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else if (projection <= -1.0f + SIMD_EPSILON) { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; }
/** Adds a successor to a node. This function will also pre-compute certain * values (like distance from this node to the successor, angle (in world) * between this node and the successor. * \param to The index of the drive node of the successor. */ void DriveNode::addSuccessor(unsigned int to) { m_successor_nodes.push_back(to); // to is the drive node DriveNode* dn_to = DriveGraph::get()->getNode(to); // Note that the first predecessor is (because of the way the drive graph // is exported) the most 'natural' one, i.e. the one on the main // driveline. dn_to->m_predecessor_nodes.push_back(m_index); Vec3 d = m_lower_center - dn_to->m_lower_center; m_distance_to_next.push_back(d.length()); Vec3 loc_pos = btTransform(shortestArcQuat(Vec3(0, 1, 0), getNormal()), getCenter()).inverse()(dn_to->getCenter()); m_angle_to_next.push_back(atan2(loc_pos.x(), loc_pos.z())); } // addSuccessor
/** The constructor stores a pointer to the kart this object is animating, * and initialised the timer. * \param kart Pointer to the kart which is animated. */ RescueAnimation::RescueAnimation(AbstractKart *kart, bool is_auto_rescue) : AbstractKartAnimation(kart, "RescueAnimation") { m_referee = new Referee(*m_kart); m_kart->getNode()->addChild(m_referee->getSceneNode()); m_timer = m_kart->getKartProperties()->getRescueDuration(); m_velocity = m_kart->getKartProperties()->getRescueHeight() / m_timer; m_xyz = m_kart->getXYZ(); m_kart->getAttachment()->clear(); // Get the current rotation of the kart m_curr_rotation = m_kart->getNode()->getRotation() * DEGREE_TO_RAD; // Determine the rotation that will rotate the kart from the current // up direction to the right up direction it should have according to // the normal at the kart's location Vec3 up = m_kart->getTrans().getBasis().getColumn(1); btQuaternion q = shortestArcQuat(up, m_kart->getNormal()); // Store this rotation as 'delta HPR', which is added over time to the // current rotation to end up (after m_timer seconds) with the right up // rotation m_add_rotation.setHPR(q); m_add_rotation /= m_timer; // Add a hit unless it was auto-rescue if(race_manager->getMinorMode()==RaceManager::MINOR_MODE_3_STRIKES && !is_auto_rescue) { ThreeStrikesBattle *world=(ThreeStrikesBattle*)World::getWorld(); world->kartHit(m_kart->getWorldKartId()); if (UserConfigParams::m_arena_ai_stats) world->increaseRescueCount(); } }; // RescueAnimation
void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) { m_swingCorrection = btScalar(0.); m_twistLimitSign = btScalar(0.); m_solveTwistLimit = false; m_solveSwingLimit = false; // compute rotation of A wrt B (in constraint space) if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) { // it is assumed that setMotorTarget() was alredy called // and motor target m_qTarget is within constraint limits // TODO : split rotation to pure swing and pure twist // compute desired transforms in world btTransform trPose(m_qTarget); btTransform trA = transA * m_rbAFrame; btTransform trB = transB * m_rbBFrame; btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); float swingAxisLen2 = swingAxis.length2(); if(btFuzzyZero(swingAxisLen2)) { return; } m_swingAxis = swingAxis; m_swingAxis.normalize(); m_swingCorrection = qDeltaAB.getAngle(); if(!btFuzzyZero(m_swingCorrection)) { m_solveSwingLimit = true; } return; } { // compute rotation of A wrt B (in constraint space) btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation(); btQuaternion qAB = qB.inverse() * qA; // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) { btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); if (swingAngle > swingLimit * m_limitSoftness) { m_solveSwingLimit = true; // compute limit ratio: 0->1, where // 0 == beginning of soft limit // 1 == hard/real limit m_swingLimitRatio = 1.f; if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) { m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ (swingLimit - swingLimit * m_limitSoftness); } // swing correction tries to get back to soft limit m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); // adjustment of swing axis (based on ellipse normal) adjustSwingAxisToUseEllipseNormal(swingAxis); // Calculate necessary axis & factors m_swingAxis = quatRotate(qB, -swingAxis); m_twistAxisA.setValue(0,0,0); m_kSwing = btScalar(1.) / (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); } } else { // you haven't set any limits; // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) // anyway, we have either hinge or fixed joint btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0); btVector3 target; btScalar x = ivB.dot(ivA); btScalar y = ivB.dot(jvA); btScalar z = ivB.dot(kvA); if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { // fixed. We'll need to add one more row to constraint if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; m_swingAxis = -ivB.cross(ivA); } } else { if(m_swingSpan1 < m_fixThresh) { // hinge around Y axis if(!(btFuzzyZero(y))) { m_solveSwingLimit = true; if(m_swingSpan2 >= m_fixThresh) { y = btScalar(0.f); btScalar span2 = btAtan2(z, x); if(span2 > m_swingSpan2) { x = btCos(m_swingSpan2); z = btSin(m_swingSpan2); } else if(span2 < -m_swingSpan2) { x = btCos(m_swingSpan2); z = -btSin(m_swingSpan2); } } } } else { // hinge around Z axis if(!btFuzzyZero(z)) { m_solveSwingLimit = true; if(m_swingSpan1 >= m_fixThresh) { z = btScalar(0.f); btScalar span1 = btAtan2(y, x); if(span1 > m_swingSpan1) { x = btCos(m_swingSpan1); y = btSin(m_swingSpan1); } else if(span1 < -m_swingSpan1) { x = btCos(m_swingSpan1); y = -btSin(m_swingSpan1); } } } } target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; target.normalize(); m_swingAxis = -ivB.cross(target); m_swingCorrection = m_swingAxis.length(); m_swingAxis.normalize(); } } if (m_twistSpan >= btScalar(0.f)) { btVector3 twistAxis; computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); if (m_twistAngle > m_twistSpan*m_limitSoftness) { m_solveTwistLimit = true; m_twistLimitRatio = 1.f; if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) { m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ (m_twistSpan - m_twistSpan * m_limitSoftness); } // twist correction tries to get back to soft limit m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); m_twistAxis = quatRotate(qB, -twistAxis); m_kTwist = btScalar(1.) / (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); } if (m_solveSwingLimit) m_twistAxisA = quatRotate(qA, -twistAxis); } else { m_twistAngle = btScalar(0.f); } } }
void btConeTwistConstraint::calcAngleInfo() { m_swingCorrection = btScalar(0.); m_twistLimitSign = btScalar(0.); m_solveTwistLimit = false; m_solveSwingLimit = false; btVector3 b1Axis1,b1Axis2,b1Axis3; btVector3 b2Axis1,b2Axis2; b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); btScalar swing1=btScalar(0.),swing2 = btScalar(0.); btScalar swx=btScalar(0.),swy = btScalar(0.); btScalar thresh = btScalar(10.); btScalar fact; // Get Frame into world space if (m_swingSpan1 >= btScalar(0.05f)) { b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); swing1 = btAtan2Fast(swy, swx); fact = (swy*swy + swx*swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); swing1 *= fact; } if (m_swingSpan2 >= btScalar(0.05f)) { b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); swing2 = btAtan2Fast(swy, swx); fact = (swy*swy + swx*swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); swing2 *= fact; } btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { m_swingCorrection = EllipseAngle-1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; } // Twist limits if (m_twistSpan >= btScalar(0.)) { btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); m_twistAngle = twist; // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); if (twist <= -m_twistSpan*lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_twistAxis *= -1.0f; } else if (twist > m_twistSpan*lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); } } }
void btFixedConstraint::getInfo2 (btConstraintInfo2* info) { //fix the 3 linear degrees of freedom const btTransform& transA = m_rbA.getCenterOfMassTransform(); const btTransform& transB = m_rbB.getCenterOfMassTransform(); const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis(); info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[info->rowskip+1] = 1; info->m_J1linearAxis[2*info->rowskip+2] = 1; btVector3 a1 = worldOrnA * m_frameInA.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); btVector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } if (info->m_J2linearAxis) { info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[info->rowskip+1] = -1; info->m_J2linearAxis[2*info->rowskip+2] = -1; } btVector3 a2 = worldOrnB*m_frameInB.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); a2.getSkewSymmetricMatrix(angular0,angular1,angular2); } // set right hand side for the linear dofs btScalar k = info->fps * info->erp; btVector3 linearError = k*(a2+worldPosB-a1-worldPosA); int j; for (j=0; j<3; j++) { info->m_constraintError[j*info->rowskip] = linearError[j]; //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0); btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1); btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2); btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0); btVector3 target; btScalar x = ivB.dot(ivA); btScalar y = ivB.dot(jvA); btScalar z = ivB.dot(kvA); btVector3 swingAxis(0,0,0); { if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) { swingAxis = -ivB.cross(ivA); } } btVector3 vTwist(1,0,0); // compute rotation of A wrt B (in constraint space) btQuaternion qA = transA.getRotation() * m_frameInA.getRotation(); btQuaternion qB = transB.getRotation() * m_frameInB.getRotation(); btQuaternion qAB = qB.inverse() * qA; // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); int row = 3; int srow = row * info->rowskip; btVector3 ax1; // angular limits { btScalar *J1 = info->m_J1angularAxis; btScalar *J2 = info->m_J2angularAxis; btTransform trA = transA*m_frameInA; btVector3 twistAxis = trA.getBasis().getColumn(0); btVector3 p = trA.getBasis().getColumn(1); btVector3 q = trA.getBasis().getColumn(2); int srow1 = srow + info->rowskip; J1[srow+0] = p[0]; J1[srow+1] = p[1]; J1[srow+2] = p[2]; J1[srow1+0] = q[0]; J1[srow1+1] = q[1]; J1[srow1+2] = q[2]; J2[srow+0] = -p[0]; J2[srow+1] = -p[1]; J2[srow+2] = -p[2]; J2[srow1+0] = -q[0]; J2[srow1+1] = -q[1]; J2[srow1+2] = -q[2]; btScalar fact = info->fps; info->m_constraintError[srow] = fact * swingAxis.dot(p); info->m_constraintError[srow1] = fact * swingAxis.dot(q); info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; info->m_lowerLimit[srow1] = -SIMD_INFINITY; info->m_upperLimit[srow1] = SIMD_INFINITY; srow = srow1 + info->rowskip; { btQuaternion qMinTwist = qABTwist; btScalar twistAngle = qABTwist.getAngle(); if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { qMinTwist = -(qABTwist); twistAngle = qMinTwist.getAngle(); } if (twistAngle > SIMD_EPSILON) { twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); twistAxis.normalize(); twistAxis = quatRotate(qB, -twistAxis); } ax1 = twistAxis; btScalar *J1 = info->m_J1angularAxis; btScalar *J2 = info->m_J2angularAxis; J1[srow+0] = ax1[0]; J1[srow+1] = ax1[1]; J1[srow+2] = ax1[2]; J2[srow+0] = -ax1[0]; J2[srow+1] = -ax1[1]; J2[srow+2] = -ax1[2]; btScalar k = info->fps; info->m_constraintError[srow] = k * twistAngle; info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } } }
void btConeTwistConstraint::buildJacobian() { m_appliedImpulse = btScalar(0.); //set bias, sign, clear accumulator m_swingCorrection = btScalar(0.); m_twistLimitSign = btScalar(0.); m_solveTwistLimit = false; m_solveSwingLimit = false; m_accTwistLimitImpulse = btScalar(0.); m_accSwingLimitImpulse = btScalar(0.); if (!m_angularOnly) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btVector3 relPos = pivotBInW - pivotAInW; btVector3 normal[3]; if (relPos.length2() > SIMD_EPSILON) { normal[0] = relPos.normalized(); } else { normal[0].setValue(btScalar(1.0),0,0); } btPlaneSpace1(normal[0], normal[1], normal[2]); for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(), normal[i], m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); } } btVector3 b1Axis1,b1Axis2,b1Axis3; btVector3 b2Axis1,b2Axis2; b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); btScalar swing1=btScalar(0.),swing2 = btScalar(0.); // Get Frame into world space if (m_swingSpan1 >= btScalar(0.05f)) { b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); } if (m_swingSpan2 >= btScalar(0.05f)) { b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); } btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { m_swingCorrection = EllipseAngle-1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); } // Twist limits if (m_twistSpan >= btScalar(0.)) { btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); if (twist <= -m_twistSpan*lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_twistAxis *= -1.0f; m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); } else if (twist > m_twistSpan*lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); } } }
bool btPolyhedralConvexShape::initializePolyhedralFeatures() { if (m_polyhedron) btAlignedFree(m_polyhedron); void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); m_polyhedron = new (mem) btConvexPolyhedron; btAlignedObjectArray<btVector3> orgVertices; for (int i=0;i<getNumVertices();i++) { btVector3& newVertex = orgVertices.expand(); getVertex(i,newVertex); } #if 0 btAlignedObjectArray<btVector3> planeEquations; btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); btAlignedObjectArray<btVector3> shiftedPlaneEquations; for (int p=0;p<planeEquations.size();p++) { btVector3 plane = planeEquations[p]; plane[3] -= getMargin(); shiftedPlaneEquations.push_back(plane); } btAlignedObjectArray<btVector3> tmpVertices; btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); btConvexHullComputer conv; conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); #else btConvexHullComputer conv; conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); #endif btAlignedObjectArray<btVector3> faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); btConvexHullComputer* convexUtil = &conv; btAlignedObjectArray<btFace> tmpFaces; tmpFaces.resize(numFaces); int numVertices = convexUtil->vertices.size(); m_polyhedron->m_vertices.resize(numVertices); for (int p=0;p<numVertices;p++) { m_polyhedron->m_vertices[p] = convexUtil->vertices[p]; } for (int i=0;i<numFaces;i++) { int face = convexUtil->faces[i]; //printf("face=%d\n",face); const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; const btConvexHullComputer::Edge* edge = firstEdge; btVector3 edges[3]; int numEdges = 0; //compute face normals // btScalar maxCross2 = 0.f; // int chosenEdge = -1; do { int src = edge->getSourceVertex(); tmpFaces[i].m_indices.push_back(src); int targ = edge->getTargetVertex(); btVector3 wa = convexUtil->vertices[src]; btVector3 wb = convexUtil->vertices[targ]; btVector3 newEdge = wb-wa; newEdge.normalize(); if (numEdges<2) edges[numEdges++] = newEdge; edge = edge->getNextEdgeOfFace(); } while (edge!=firstEdge); btScalar planeEq = 1e30f; if (numEdges==2) { faceNormals[i] = edges[0].cross(edges[1]); faceNormals[i].normalize(); tmpFaces[i].m_plane[0] = faceNormals[i].getX(); tmpFaces[i].m_plane[1] = faceNormals[i].getY(); tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); tmpFaces[i].m_plane[3] = planeEq; } else { btAssert(0);//degenerate? faceNormals[i].setZero(); } for (int v=0;v<tmpFaces[i].m_indices.size();v++) { btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); if (planeEq>eq) { planeEq=eq; } } tmpFaces[i].m_plane[3] = -planeEq; } //merge coplanar faces and copy them to m_polyhedron btScalar faceWeldThreshold= 0.999f; btAlignedObjectArray<int> todoFaces; for (int i=0;i<tmpFaces.size();i++) todoFaces.push_back(i); while (todoFaces.size()) { btAlignedObjectArray<int> coplanarFaceGroup; int refFace = todoFaces[todoFaces.size()-1]; coplanarFaceGroup.push_back(refFace); btFace& faceA = tmpFaces[refFace]; todoFaces.pop_back(); btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); for (int j=todoFaces.size()-1;j>=0;j--) { int i = todoFaces[j]; btFace& faceB = tmpFaces[i]; btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) { coplanarFaceGroup.push_back(i); todoFaces.remove(i); } } if (coplanarFaceGroup.size()>1) { //do the merge: use Graham Scan 2d convex hull btAlignedObjectArray<GrahamVector2> orgpoints; for (int i=0;i<coplanarFaceGroup.size();i++) { // m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); btFace& face = tmpFaces[coplanarFaceGroup[i]]; btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); btVector3 xyPlaneNormal(0,0,1); btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal); for (int f=0;f<face.m_indices.size();f++) { int orgIndex = face.m_indices[f]; btVector3 pt = m_polyhedron->m_vertices[orgIndex]; btVector3 rotatedPt = quatRotate(rotationArc,pt); rotatedPt.setZ(0); bool found = false; for (int i=0;i<orgpoints.size();i++) { if ((rotatedPt-orgpoints[i]).length2()<0.001) { found=true; break; } } if (!found) orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex)); } } btFace combinedFace; for (int i=0;i<4;i++) combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; btAlignedObjectArray<GrahamVector2> hull; GrahamScanConvexHull2D(orgpoints,hull); for (int i=0;i<hull.size();i++) { combinedFace.m_indices.push_back(hull[i].m_orgIndex); } m_polyhedron->m_faces.push_back(combinedFace); } else { for (int i=0;i<coplanarFaceGroup.size();i++) { m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); } } } m_polyhedron->initialize(); return true; }
bool btConvexUtility::initializePolyhedralFeatures(const btAlignedObjectArray<btVector3>& orgVertices, bool mergeCoplanarTriangles) { btConvexHullComputer conv; conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); btAlignedObjectArray<btVector3> faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); btConvexHullComputer* convexUtil = &conv; btAlignedObjectArray<btFace> tmpFaces; tmpFaces.resize(numFaces); int numVertices = convexUtil->vertices.size(); m_vertices.resize(numVertices); for (int p=0;p<numVertices;p++) { m_vertices[p] = convexUtil->vertices[p]; } for (int i=0;i<numFaces;i++) { int face = convexUtil->faces[i]; //printf("face=%d\n",face); const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; const btConvexHullComputer::Edge* edge = firstEdge; btVector3 edges[3]; int numEdges = 0; //compute face normals btScalar maxCross2 = 0.f; int chosenEdge = -1; do { int src = edge->getSourceVertex(); tmpFaces[i].m_indices.push_back(src); int targ = edge->getTargetVertex(); btVector3 wa = convexUtil->vertices[src]; btVector3 wb = convexUtil->vertices[targ]; btVector3 newEdge = wb-wa; newEdge.normalize(); if (numEdges<2) edges[numEdges++] = newEdge; edge = edge->getNextEdgeOfFace(); } while (edge!=firstEdge); btScalar planeEq = 1e30f; if (numEdges==2) { faceNormals[i] = edges[0].cross(edges[1]); faceNormals[i].normalize(); tmpFaces[i].m_plane[0] = faceNormals[i].getX(); tmpFaces[i].m_plane[1] = faceNormals[i].getY(); tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); tmpFaces[i].m_plane[3] = planeEq; } else { btAssert(0);//degenerate? faceNormals[i].setZero(); } for (int v=0;v<tmpFaces[i].m_indices.size();v++) { btScalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); if (planeEq>eq) { planeEq=eq; } } tmpFaces[i].m_plane[3] = -planeEq; } //merge coplanar faces btScalar faceWeldThreshold= 0.999f; btAlignedObjectArray<int> todoFaces; for (int i=0;i<tmpFaces.size();i++) todoFaces.push_back(i); while (todoFaces.size()) { btAlignedObjectArray<int> coplanarFaceGroup; int refFace = todoFaces[todoFaces.size()-1]; coplanarFaceGroup.push_back(refFace); btFace& faceA = tmpFaces[refFace]; todoFaces.pop_back(); btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); for (int j=todoFaces.size()-1;j>=0;j--) { int i = todoFaces[j]; btFace& faceB = tmpFaces[i]; btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) { coplanarFaceGroup.push_back(i); todoFaces.remove(i); } } bool did_merge = false; if (mergeCoplanarTriangles && coplanarFaceGroup.size()>1) { //do the merge: use Graham Scan 2d convex hull btAlignedObjectArray<GrahamVector2> orgpoints; for (int i=0;i<coplanarFaceGroup.size();i++) { btFace& face = tmpFaces[coplanarFaceGroup[i]]; btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); btVector3 xyPlaneNormal(0,0,1); btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal); for (int f=0;f<face.m_indices.size();f++) { int orgIndex = face.m_indices[f]; btVector3 pt = m_vertices[orgIndex]; btVector3 rotatedPt = quatRotate(rotationArc,pt); rotatedPt.setZ(0); bool found = false; for (int i=0;i<orgpoints.size();i++) { //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) if (orgpoints[i].m_orgIndex == orgIndex) { found=true; break; } } if (!found) orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex)); } } btFace combinedFace; for (int i=0;i<4;i++) combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; btAlignedObjectArray<GrahamVector2> hull; GrahamScanConvexHull2D(orgpoints,hull); for (int i=0;i<hull.size();i++) { combinedFace.m_indices.push_back(hull[i].m_orgIndex); for(int k = 0; k < orgpoints.size(); k++) { if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) { orgpoints[k].m_orgIndex = -1; // invalidate... break; } } } // are there rejected vertices? bool reject_merge = false; for(int i = 0; i < orgpoints.size(); i++) { if(orgpoints[i].m_orgIndex == -1) continue; // this is in the hull... // this vertex is rejected -- is anybody else using this vertex? for(int j = 0; j < tmpFaces.size(); j++) { btFace& face = tmpFaces[j]; // is this a face of the current coplanar group? bool is_in_current_group = false; for(int k = 0; k < coplanarFaceGroup.size(); k++) { if(coplanarFaceGroup[k] == j) { is_in_current_group = true; break; } } if(is_in_current_group) // ignore this face... continue; // does this face use this rejected vertex? for(int v = 0; v < face.m_indices.size(); v++) { if(face.m_indices[v] == orgpoints[i].m_orgIndex) { // this rejected vertex is used in another face -- reject merge reject_merge = true; break; } } if(reject_merge) break; } if(reject_merge) break; } if(!reject_merge) { // do this merge! did_merge = true; m_faces.push_back(combinedFace); } } if(!did_merge) { for (int i=0;i<coplanarFaceGroup.size();i++) { m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); } } } return true; }