MatrixF PlaneReflector::getCameraReflection( const MatrixF &camTrans ) { Point3F normal = refplane; // Figure out new cam position Point3F camPos = camTrans.getPosition(); F32 dist = refplane.distToPlane( camPos ); Point3F newCamPos = camPos - normal * dist * 2.0; // Figure out new look direction Point3F i, j, k; camTrans.getColumn( 0, &i ); camTrans.getColumn( 1, &j ); camTrans.getColumn( 2, &k ); i = MathUtils::reflect( i, normal ); j = MathUtils::reflect( j, normal ); k = MathUtils::reflect( k, normal ); //mCross( i, j, &k ); MatrixF newTrans(true); newTrans.setColumn( 0, i ); newTrans.setColumn( 1, j ); newTrans.setColumn( 2, k ); newTrans.setPosition( newCamPos ); return newTrans; }
void SelfLocator::update(RobotPoseHypotheses& robotPoseHypotheses) { robotPoseHypotheses.hypotheses.clear(); //update only available for two types of pose calculation: if(poseCalculatorType != POSE_CALCULATOR_PARTICLE_HISTORY && poseCalculatorType != POSE_CALCULATOR_K_MEANS_CLUSTERING) return; //sample set needs to have been updated within this frame: if(lastPoseComputationTimeStamp != bb->theFrameInfo.time) { RobotPose dummyPose; update(dummyPose); } if(poseCalculatorType == POSE_CALCULATOR_PARTICLE_HISTORY) { //get hypotheses: PoseCalculatorParticleHistory< Sample, SampleSet<Sample> >* poseHistoryCalc = (PoseCalculatorParticleHistory< Sample, SampleSet<Sample> >*)poseCalculator; vector< pair<int, int> > clusters = poseHistoryCalc->getClusters(); sort(clusters.begin(), clusters.end(), cmpClusterPairs); const unsigned int MAX_HYPOTHESES = min<unsigned int>(robotPoseHypotheses.MAX_HYPOTHESES, clusters.size()); for(unsigned int i = 0; i < MAX_HYPOTHESES; ++i) { // No mini clusters, please: if(clusters[i].second <= 3) break; // Compute average position: RobotPoseHypothesis newHypothesis; poseHistoryCalc->calcPoseOfCluster(newHypothesis, clusters[i].first); Vector2<int> newTrans(static_cast<int>(newHypothesis.translation.x), static_cast<int>(newHypothesis.translation.y)); // Compute variance of position: int varianceX(0); int varianceY(0); for(int j = 0; j < samples->size(); ++j) { Sample& s(samples->at(j)); if(s.cluster == clusters[i].first) { varianceX += (s.translation.x - newTrans.x) * (s.translation.x - newTrans.x); varianceY += (s.translation.y - newTrans.y) * (s.translation.y - newTrans.y); } } varianceX /= (clusters[i].second - 1); varianceY /= (clusters[i].second - 1); newHypothesis.positionCovariance[0][0] = static_cast<float>(varianceX); newHypothesis.positionCovariance[1][1] = static_cast<float>(varianceY); // Compute covariance: int cov_xy(0); for(int j = 0; j < samples->size(); ++j) { Sample& s(samples->at(j)); if(s.cluster == clusters[i].first) cov_xy += (s.translation.x - newTrans.x) * (s.translation.y - newTrans.y); } cov_xy /= (clusters[i].second - 1); newHypothesis.positionCovariance[0][1] = newHypothesis.positionCovariance[1][0] = static_cast<float>(cov_xy); // Finally add to list: robotPoseHypotheses.hypotheses.push_back(newHypothesis); } } if(poseCalculatorType == POSE_CALCULATOR_K_MEANS_CLUSTERING) { //get hypotheses: PoseCalculatorKMeansClustering< Sample, SampleSet<Sample>, 5, 1000 >* poseKMeansCalc = (PoseCalculatorKMeansClustering< Sample, SampleSet<Sample>, 5, 1000 >*)poseCalculator; for(unsigned int i = 0; i < 5; ++i) { RobotPoseHypothesis newHypothesis; newHypothesis.validity = poseKMeansCalc->getClusterValidity(i); if(newHypothesis.validity > 0) { poseKMeansCalc->getClusterPose(newHypothesis, i); newHypothesis.positionCovariance[0][0] = 0.1f; //TODO: calculate covariances newHypothesis.positionCovariance[0][1] = 0.1f; newHypothesis.positionCovariance[1][0] = 0.1f; newHypothesis.positionCovariance[1][1] = 0.1f; robotPoseHypotheses.hypotheses.push_back(newHypothesis); } } } }
void CalBone::blendState(float unrampedWeight, const CalVector& translation, const CalQuaternion& rotation, float scale, bool replace, float rampValue, bool absoluteTranslation ) { // Attenuate the weight by the accumulated replacement attenuation. Each applied // "replacement" animation attenuates the weights of the subsequent animations by // the inverse of its rampValue, so that when a replacement animation ramps up to // full, all lesser priority animations automatically ramp down to zero. float rampedWeight = unrampedWeight * rampValue; float attenuatedWeight = rampedWeight * m_accumulatedReplacementAttenuation; // It appears that quaternion::blend() only works with blend factors of 0-1, so // I'll clamp the scale to that range. if( scale < 0.0f ) { scale = 0.0f; } if( scale > 1.0f ) { scale = 1.0f; } // Now apply weighted, scaled transformation. For weights, Cal starts with the // first and then blends the later ones in proportion to their weights. Though this // would seem to depend on the order, you can reason by induction that it does not. // Each application of an animation gives it the correct proportion to the others in // aggregate and leaves in tact the proportions among the others. if( m_accumulatedWeightAbsolute == 0.0f ) { // It is the first state, so we can just copy it into the bone state. The first animation // must be applied with scale = 1.0 since it is the initial pose rather than something // to be blended onto a pose. If we scale the first state, the skeleton will look like // a crumpled spider. m_accumulatedWeightAbsolute = attenuatedWeight; m_translationAbsolute = absoluteTranslation ? translation : m_translation + translation; m_rotationAbsolute = rotation; // I would like to scale this blend, but I cannot since it is the initial pose. Thus I // will store away this scale and compensate appropriately on the second blend. See below. // After applying blend2, the blend1 = 1 - blend2. If I would like to scale blend1 to 30% // of its original scale, for example, then I would like, // // ( 1 - blend2' ) = 0.3 * ( 1 - blend2 ) // so, // blend2' = 1 - 0.3 * ( 1 - blend2 ) // // or similarly for any value of m_firstBlendScale instead of 30%. m_firstBlendScale = scale; } else { // Consider an example with two animations, one or both of them "replace" animations. // Wave is a "replace" animation, played on top of Walk. Wave is applied first since it is a // "replace" animation and Walk is not. Imagine Wave is ramping in, currently at 80%. Wave sets // the initial pose 100% and then Walk is applied over that pose with a blend factor of 0.2. The result // is that Wave is 80% and Walk is 20%, which is what you'd expect for replace semantics. // // Animation RampedWeight AttenuatedWeight InAccumWeightAbs OutAccAttenuation Factor // Wave 0.8 0.8 0.0 0.2 (replace) n/a (100%) // Walk 1.0 0.2 0.8 0.2 (not replace) 0.2/(0.8+0.2) = 0.2 // // Consider the same example with two animations, but neither of them "replace" animations. // Assume Wave is applied first. Imagine Wave is ramping in, currently at 80%. Wave sets // the initial pose 100% and then Walk is applied over that pose with a blend factor of 0.55. The result // is that Wave is 45% and Walk is 55%, which is about what you'd expect for non-replace semantics. // // Animation RampedWeight AttenuatedWeight InAccumWeightAbs OutAccAttenuation Factor // Wave 0.8 0.8 0.0 1.0 (not replace) n/a (100%) // Walk 1.0 1.0 0.8 1.0 (not replace) 1.0/(0.8+1.0) = 0.55 // // Consider the same example again but reverse the order of Wave and Walk, so Walk is applied first. // As before, imagine Wave is ramping in, currently at 80%. Walk sets the initial pose 100% // and then Wave is applied over that pose with a blend factor of 0.44. The result // is that Wave is 44% and Walk is 56%, which is also about what you'd expect for non-replace semantics. // // Animation RampedWeight AttenuatedWeight InAccumWeightAbs OutAccAttenuation Factor // Walk 1.0 1.0 0.0 1.0 (not replace) n/a (100%) // Wave 0.8 0.8 1.0 1.0 (not replace) 0.8/(0.8+1.0) = 0.44 // // Now consider an example in which Point and Wave are both applied over Walk, with Point applied // first at highest priority. Assume that Point is ramped at 90% and Wave is ramped at 80%. Both // Point and Wave are "replace" animations. Walk is not. The result is Walk is 2%, Wave is about 8%, // and Point is about 90%, which seems like a reasonable result. // // Animation RampedWeight AttenuatedWeight InAccumWeightAbs OutAccAttenuation Factor // Point 0.9 0.9 0 0.1 (replace) n/a (100%) // Wave 0.8 0.08 0.9 0.02 (replace) 0.08/(0.9+0.08) = 0.082 // Walk 1.0 0.02 0.98 0.02 (not replace) 0.02/(0.98+0.02) = 0.02 // // Finally, consider an example in which Point and Wave are both applied over Walk, but in which // none of the animations is a "replace" animation. For this example, assume that Point, Wave, // and Walk all are fully ramped in at 100%. The result is Walk is 33%, Wave is about 33%, // and Point is about 33%, which seems like the right result. // // Animation RampedWeight AttenuatedWeight InAccumWeightAbs OutAccAttenuation Factor // Point 1.0 1.0 0.0 1.0 (not replace) n/a (100%) // Wave 1.0 1.0 1.0 1.0 (not replace) 1.0/(1.0+1.0) = 0.5 // Walk 1.0 1.0 2.0 1.0 (not replace) 1.0/(1.0+2.0) = 0.33 float factor = scale * attenuatedWeight / ( m_accumulatedWeightAbsolute + attenuatedWeight ); // If the scale of the first blend was not 1.0, then I will adjust the factor of the second blend // to compensate, // // factor' = 1 - m_firstBlendScale * ( 1 - factor ) // assert( factor <= 1.0f ); factor = 1.0f - m_firstBlendScale * ( 1.0f - factor ); CalVector newTrans(absoluteTranslation ? translation : m_translation + translation); m_translationAbsolute.blend(factor, newTrans); m_rotationAbsolute.blend(factor, rotation); m_accumulatedWeightAbsolute += attenuatedWeight; m_firstBlendScale = 1.0; } if( replace ) { m_accumulatedReplacementAttenuation *= ( 1.0f - rampValue ); } }