shape_t
shape
(
)const
/**@brief
* Returns shape(i.e. lengths of axes).
*/
{
shape_t shape_v(rank());
if(my_dir==dir_fwd)
{
iter_shape
( my_strides.begin()
, shape_v.begin()
, shape_v.end()
);
}
else
{
iter_shape
( my_strides.rbegin()
, shape_v.rbegin()
, shape_v.rend()
);
}
#ifdef SHAPE_ALL_SAME_SHAPE_ONES
bool ok=true;
index_t ishape=0;
for(; ishape<rank() && ok; ++ishape)
{
ok=shape_v[ishape]==size(ishape);
if(!ok)
{
std::cout<<"inconsistent shape() vs size(ishape)@ishape="<<ishape
<<":shape[ishape]="<<shape_v[ishape]
<<":size(ishape)="<<size(ishape)
<<"\n";
}
}
#endif
return shape_v;
}
void rot_block(struct game_state *gs)
{
int x0 = gs->x;
int y0 = gs->y;
int r0 = gs->rot;
int r1 = (r0 + 1) % 4;
gs->rot = r1;
if (!fit_block(gs)) {
r1 = r0;
}
int k = 0;
gs->rot = r0;
iter_shape(gs, x0, y0, const_modf, &k);
gs->rot = r1;
put_shape(gs, 4);
}
void RigidBodyConnectedVisibilityCanon::ComputeApproxMeanShape(RigidBodyShape& mean) const
{
if (shape.size())
{
// iterator at start of list
std::vector<RigidBodyShape>::const_iterator iter_shape( shape.begin() );
// initialise mean to first shape
mean = *iter_shape;
// add all others
while ((++iter_shape) != shape.end())
{
// fit this shape to mean
RigidTransform3 T;
UInt32 fit_result = iter_shape->ComputeFitTo(T, mean);
// this if statement is just defensive code
// - in fact the nature of the canon should guarantee sufficient points for a unique fit
if (fit_result == RigidBodyResult::success)
{
// loop over markers
for (size_t marker_index = 0; marker_index < mean.NumMarkers(); marker_index++)
{
const RigidBodyMarker& sample_marker = iter_shape->Marker(marker_index);
RigidBodyMarker& mean_marker = mean.Marker(marker_index);
if (!mean_marker.visible && sample_marker.visible)
{
// transform marker from this shape to mean
Vector3 Tx;
RigidTransform3::MulVec(Tx, T, sample_marker.position);
// fill in mean
mean_marker.position = Tx;
mean_marker.visible = true;
}
}
}
}
}
}
void RigidBodyShapeCollection::ComputeVisiblityCanon(RigidBodyVisibilityCanon& canon) const
{
std::vector< std::vector<RigidBodyShape>::const_iterator > canon_indices;
for (std::vector<RigidBodyShape>::const_iterator iter_shape( shape.begin() ); iter_shape != shape.end(); iter_shape++)
{
bool isnotsubcode = true;
for (std::vector< std::vector<RigidBodyShape>::const_iterator >::iterator iter_canon( canon_indices.begin() ); iter_canon != canon_indices.end(); iter_canon++)
{
if (*iter_canon != shape.end())
{
if (iter_shape->IsVisibilitySubsetOf( **iter_canon ))
{
isnotsubcode = false;
break;
}
if (iter_shape->IsVisibilitySupersetOf( **iter_canon ))
{
*iter_canon = shape.end();
}
}
}
if (isnotsubcode)
{
canon_indices.push_back( iter_shape );
}
}
canon.shape.clear();
for (std::vector< std::vector<RigidBodyShape>::const_iterator >::iterator iter_canon( canon_indices.begin() ); iter_canon != canon_indices.end(); iter_canon++)
{
if (*iter_canon != shape.end())
{
canon.shape.push_back( **iter_canon );
}
}
}
UInt32 RigidBodyShapeCollection::CopyFromTimeSequences(const std::vector<const TimeSequence*>& input)
{
try
{
// clear any previous
shape.clear();
// copy if non-empty
size_t num_markers = input.size();
if (num_markers)
{
size_t num_frames = (size_t) input[0]->NumFrames();
shape.resize( num_frames );
for ( std::vector<const TimeSequence*>::const_iterator iter_input( input.begin() ); iter_input != input.end(); iter_input++)
{
if ((size_t)(*iter_input)->NumFrames() == num_frames)
{
std::vector<RigidBodyShape>::iterator iter_shape( shape.begin() );
for (TSOccVector3ConstIter iter_marker(**iter_input); iter_marker.HasFrame(); iter_marker.Next(), iter_shape++)
{
iter_shape->AddMarker(iter_marker.Value(), iter_marker.Occluded() ? 0 : 1);
}
}
else
{
return RigidBodyResult::timesequence_mismatch;
}
}
}
return RigidBodyResult::success;
}
catch (const NoSuchFieldException&)
{
return RigidBodyResult::timesequence_mismatch;
}
}
UInt32 RigidBodyShapeCollection::ComputeMeanShape(RigidBodyShape& mean, double distribution_tolerance, double convergence_accuracy) const
{
// minimal set of visiblity codes
RigidBodyVisibilityCanon canon;
ComputeVisiblityCanon(canon);
// re-order codes to ensure maximal ones go first
RigidBodyConnectedVisibilityCanon connected_canon;
UInt32 canon_result = canon.ComputeConnectedVisibilityCanon(connected_canon, distribution_tolerance);
// must be a complete covering of all points to work
if (canon_result != RigidBodyResult::success)
return canon_result;
// initialise shape
connected_canon.ComputeApproxMeanShape(mean);
// number of points
size_t num_markers = connected_canon.Shape(0).NumMarkers();
// zero vector to init markers
Vector3 zero(0.0);
// transform every shape to this one and update mean
// double previous_residual_rms = DBL_MAX;
for (size_t opt_count = 0; opt_count < 100; opt_count++)
{
// transform every shape and sum
std::vector<Vector3> point_sum(num_markers, zero);
std::vector<size_t> point_count(num_markers, 0);
// double dx2_sum = 0.0;
// size_t dx2_count = 0;
for (std::vector<RigidBodyShape>::const_iterator iter_shape( shape.begin() ); iter_shape != shape.end(); iter_shape++)
{
RigidTransform3 T;
iter_shape->ComputeFitTo(T, mean);
for (size_t marker_index = 0; marker_index < num_markers; marker_index++)
{
const RigidBodyMarker& x = iter_shape->Marker(marker_index);
if (x.visible)
{
// transform point to be nearest to mean as we can
Vector3 Tx;
RigidTransform3::MulVec(Tx, T, x.position);
point_sum[marker_index] += Tx;
point_count[marker_index]++;
// form sum residuals
// Vector3 dx;
// Vector3::Sub(dx, Tx, x.position);
// dx2_sum += dx.Modulus2();
// dx2_count++;
}
}
}
// update mean and compute max difference with previous
double max_difference = 0.0;
for (size_t marker_index = 0; marker_index < num_markers; marker_index++)
{
size_t n = point_count[marker_index];
if (n)
{
// compute updated marker position
Vector3& new_position = point_sum[marker_index];
new_position /= n;
// compute maximum coordinate difference with previous
RigidBodyMarker& m = mean.Marker(marker_index);
for (size_t dimension = 0; dimension < 3; dimension++)
{
double d = fabs(new_position[dimension] - m.position[dimension]);
if (d > max_difference)
max_difference = d;
}
// set new values
m.position = new_position;
}
}
// verify residuals as stopping criteria
// double residual_rms = sqrt( dx2_sum / dx2_count );
if (max_difference < convergence_accuracy)
{
// fprintf(stderr, "Iterations used: %u\n", opt_count);
return RigidBodyResult::success;
}
// previous_residual_rms = residual_rms;
}
return RigidBodyResult::did_not_converge;
}
void put_shape(struct game_state *gs, int k)
{
iter_shape(gs, gs->x, gs->y, const_modf, &k);
}
int collision(struct game_state *gs, int xinc, int yinc)
{
int s = 0;
return !iter_shape(gs, gs->x + xinc, gs->y + yinc, orf, &s) || s;
}
