void BattleOperationAttack::Update(void){
if(_owner == nullptr){return;}
//target生存確認
if(_target == nullptr){return;}
if(!_accessor->IsEnemy(_target)){return;}
if(_target->IsDead()){return;}
//owner行動中チェック
if(_owner->IsActing()){return;}
//距離チェック
D3DXVECTOR3 owner_position(_owner->GetPosition());
D3DXVECTOR3 target_position(_target->GetPosition());
D3DXVECTOR3 to_target(target_position - owner_position);
float sq_length(D3DXVec3Dot(&to_target, &to_target));
const float RANGE(20.0f);
if(sq_length > RANGE * RANGE){
//移動命令
D3DXVECTOR3 direction(to_target);
D3DXVec3Normalize(&direction, &direction);
float length(sqrt(sq_length) - RANGE);
D3DXVECTOR3 dest_position(owner_position + direction * length);
_owner->SetAction(new BattleActionWalk(_owner, dest_position));
return;
}
//方向チェック
//攻撃命令
_owner->SetSkill(new BattleSkillAttack(_owner, _renderer, _accessor));
_owner->SetAction(new BattleActionSkill(_owner));
}
pybind::tuple PathFinderWayAffector::predictionParabolicBullet( const mt::vec3f & _offset, const mt::vec3f & _position, const mt::vec3f & _height, float _speed ) const
{
mt::vec3f unit_start_position = m_node->getLocalPosition();
unit_start_position += _offset;
mt::vec3f parabolic_begin = _position;
mt::vec3f parabolic_end = unit_start_position;
mt::vec3f parabolic_height = (_position + unit_start_position) * 0.5f + _height;
float length = calculateParabolicLength( parabolic_begin, parabolic_end, parabolic_height, mt::length_v3_v3 );
float time = (length / _speed) * 1000.f;
const uint32_t max_iteration = 10;
float unit_time = 0.f;
mt::vec3f target_position( 0.f, 0.f, 0.f );
float bullet_time = 0.f;
const float dt = time / float( max_iteration );
for( uint32_t index = 1; index != max_iteration; ++index )
{
float test_time = unit_time + dt;
mt::vec3f test_target_position = this->getTimePosition( test_time );
test_target_position += _offset;
mt::vec3f parabolic_begin_dt = _position;
mt::vec3f parabolic_end_dt = target_position;
mt::vec3f parabolic_height_dt = (_position + test_target_position) * 0.5f + _height;
float dlength = calculateParabolicLength( parabolic_begin_dt, parabolic_end_dt, parabolic_height_dt, mt::length_v3_v3 );
float test_bullet_time = (dlength / _speed) * 1000.f;
if( test_time >= test_bullet_time )
{
break;
}
unit_time = test_time;
bullet_time = test_bullet_time;
target_position = test_target_position;
}
const float dt2 = dt / float( max_iteration );
for( uint32_t index = 1; index != max_iteration; ++index )
{
float test_time = unit_time + dt2;
mt::vec3f test_target_position = this->getTimePosition( test_time );
test_target_position += _offset;
mt::vec3f parabolic_begin = _position;
mt::vec3f parabolic_end = target_position;
mt::vec3f parabolic_height = (_position + test_target_position) * 0.5f + _height;
float dlength = calculateParabolicLength( parabolic_begin, parabolic_end, parabolic_height, mt::length_v3_v3 );
float test_bullet_time = (dlength / _speed) * 1000.f;
if( test_time >= test_bullet_time )
{
break;
}
unit_time = test_time;
bullet_time = test_bullet_time;
target_position = test_target_position;
}
float delay = unit_time > bullet_time ? unit_time - bullet_time : 0.f;
return pybind::make_tuple_t( unit_time, delay, target_position );
}
pybind::tuple PathFinderWayAffector::predictionLinearBullet( const mt::vec3f & _offset, const mt::vec3f & _position, float _speed ) const
{
mt::vec3f unit_start_position = m_node->getLocalPosition();
unit_start_position += _offset;
float length = mt::length_v3_v3( _position, unit_start_position );
float time = (length / _speed) * 1000.f;
const uint32_t max_iteration = 10;
float unit_time = 0.f;
mt::vec3f target_position( 0.f, 0.f, 0.f );
float bullet_time = 0.f;
const float dt = time / float( max_iteration );
for( uint32_t index = 1; index != max_iteration; ++index )
{
float test_time = unit_time + dt;
mt::vec3f test_target_position = this->getTimePosition( test_time );
test_target_position += _offset;
float dlength = mt::length_v3_v3( _position, test_target_position );
float test_bullet_time = (dlength / _speed) * 1000.f;
if( test_time >= test_bullet_time )
{
break;
}
unit_time = test_time;
bullet_time = test_bullet_time;
target_position = test_target_position;
}
const float dt2 = dt / float( max_iteration );
for( uint32_t index = 1; index != max_iteration; ++index )
{
float test_time = unit_time + dt2;
mt::vec3f test_target_position = this->getTimePosition( unit_time );
test_target_position += _offset;
float dlength = mt::length_v3_v3( _position, test_target_position );
float test_bullet_time = (dlength / _speed) * 1000.f;
if( test_time >= test_bullet_time )
{
break;
}
unit_time = test_time;
bullet_time = test_bullet_time;
target_position = test_target_position;
}
float delay = unit_time - bullet_time;
return pybind::make_tuple_t( unit_time, delay, target_position );
}
typename PointCloud<PointT>::Ptr transform(
const typename PointCloud<PointT>::Ptr source,
const typename PointCloud<PointT>::Ptr target,boost::shared_ptr<Configuration> configuration)
{
source_cloud = *source;
target_cloud = *target;
cout << "[PFHTransformStrategy::transform] Setting source cloud: "
<< source->size() << " points" << endl;
cout << "[PFHTransformStrategy::transform] Setting target cloud: "
<< target->size() << " points" << endl;
typename PointCloud<PointT>::Ptr transformed(new PointCloud<PointT>);
typename PointCloud<PointXYZRGB>::Ptr source_points(source);
typename PointCloud<PointXYZRGB>::Ptr source_filtered(
new PointCloud<PointT>);
PointCloud<Normal>::Ptr source_normals(new PointCloud<Normal>);
PointCloud<PointWithScale>::Ptr source_keypoints(
new PointCloud<PointWithScale>);
PointCloud<PFHSignature125>::Ptr source_descriptors(
new PointCloud<PFHSignature125>);
typename PointCloud<PointT>::Ptr target_points(target);
typename PointCloud<PointT>::Ptr target_filtered(
new PointCloud<PointT>);
PointCloud<Normal>::Ptr target_normals(
new PointCloud<Normal>);
PointCloud<PointWithScale>::Ptr target_keypoints(
new PointCloud<PointWithScale>);
PointCloud<PFHSignature125>::Ptr target_descriptors(
new PointCloud<PFHSignature125>);
cout << "[PFHTransformStrategy::transform] Downsampling source and "
<< "target clouds..." << endl;
//filter(source_points, source_filtered, 0.01f);
//filter(target_points, target_filtered, 0.01f);
source_filtered=source_points;
target_filtered=target_points;
cout << "[PFHTransformStrategy::transform] Creating normals for "
<< "source and target cloud..." << endl;
create_normals<Normal>(source_filtered, source_normals);
create_normals<Normal>(target_filtered, target_normals);
cout << "[PFHTransformStrategy::transform] Finding keypoints in "
<< "source and target cloud..." << endl;
detect_keypoints(source_filtered, source_keypoints);
detect_keypoints(target_filtered, target_keypoints);
for(PointWithScale p: source_keypoints->points){
cout<<"keypoint "<<p;
}
vector<int> source_indices(source_keypoints->points.size());
vector<int> target_indices(target_keypoints->points.size());
cout << "[PFHTransformStrategy::transform] Computing PFH features "
<< "for source and target cloud..." << endl;
compute_PFH_features_at_keypoints(source_filtered, source_normals,source_keypoints,
source_descriptors, target_indices);
compute_PFH_features_at_keypoints(target_filtered, target_normals,target_keypoints,
target_descriptors, target_indices);
vector<int> correspondences;
vector<float> correspondence_scores;
find_feature_correspondence(source_descriptors, target_descriptors,
correspondences, correspondence_scores);
cout << "correspondences: " << correspondences.size() << endl;
cout << "c. scores: " << correspondence_scores.size() << endl;
cout << "First cloud: Found " << source_keypoints->size() << " keypoints "
<< "out of " << source_filtered->size() << " total points." << endl;
cout << "Second cloud: Found " << target_keypoints->size() << " keypoints"
<< " out of " << target_filtered->size() << " total points." << endl;
// Start with the actual transformation. Yeay :)
TransformationFromCorrespondences tfc;
tfc.reset();
vector<int> sorted_scores;
cv::sortIdx(correspondence_scores, sorted_scores, CV_SORT_EVERY_ROW);
vector<float> tmp(correspondence_scores);
sort(tmp.begin(), tmp.end());
float median_score = tmp[tmp.size() / 2];
vector<int> fidx;
vector<int> fidxt;
Eigen::Vector3f source_position(0, 0, 0);
Eigen::Vector3f target_position(0, 0, 0);
for (size_t i = 0; i < correspondence_scores.size(); i++) {
int index = sorted_scores[i];
if (median_score >= correspondence_scores[index]) {
source_position[0] = source_keypoints->points[index].x;
source_position[1] = source_keypoints->points[index].y;
source_position[2] = source_keypoints->points[index].z;
target_position[0] = target_keypoints->points[index].x;
target_position[1] = target_keypoints->points[index].y;
target_position[2] = target_keypoints->points[index].z;
if (abs(source_position[1] - target_position[1]) > 0.2) {
continue;
}
// cout<< "abs position difference:"<<abs(source_position[1] - target_position[1])<<"C-Score"<<correspondence_scores[index]<<endl;
// cout<<" Source Position " <<source_position<<endl;
// cout<<" target position "<<target_position<<endl;
tfc.add(source_position, target_position,
correspondence_scores[index]);
fidx.push_back(source_indices[index]);
fidxt.push_back(target_indices[correspondences[index]]);
}
}
cout << "TFC samples: "<<tfc.getNoOfSamples()<<" AccumulatedWeight "<<tfc.getAccumulatedWeight()<<endl;
Eigen::Affine3f tr;
tr = tfc.getTransformation();
cout << "TFC transformation: " << endl;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
cout << tr.rotation()(i, i) << "\t";
}
cout << endl;
}
transformPointCloud(*source_filtered, *transformed,
tfc.getTransformation());
cout << "transformation finished";
return transformed;
};