void randomSet(Set* s) {
Set t;
int n = rand() % maximum_set_size + 1;
int i;
createEmptySet(&t);
for (i = 0; i < n; i += 1) {
insertSet(&t, rand() % maximum_set_size);
}
assignSet(s, &t);
destroySet(&t);
}
void relationalTests() {
Set s;
Set t;
int i;
createEmptySet(&s);
createEmptySet(&t);
for (i = 0; i < number_of_tests; i += 1) {
randomSet(&s);
assignSet(&t, &s);
assert(isSubsetOf(&s, &t));
assert(isSubsetOf(&t, &s));
assert(isEqualToSet(&s, &t));
assert(isEqualToSet(&t, &s));
insertSet(&s, rand() % maximum_set_size + maximum_set_size);
assert(isSubsetOf(&t, &s));
assert(! isSubsetOf(&s, &t));
}
printf("The relational tests have been passed\n");
destroySet(&s);
destroySet(&t);
}
static unsigned int makeSet_auto(AS_phasorSet *Setp, const AS_shaper *shp, unsigned int *astsp, double SxDiff, double ext_match, double uc,
double amaxH, double amaxS, double umaxH, double umaxS, double umin, double omega, int verbose, int overlap)
{
unsigned int sts=0;
double uCmd, Sxad, sfact, ext_a, phi_d, ext_travel, ext_min, uCmd_minext;
AS_phasorSet aSet;
AS_phasorSet dSet;
aSet.ph=NULL;
AS_emptySet(&aSet);
dSet.ph=NULL;
AS_emptySet(&dSet);
AS_collapseSet(Setp, shp, amaxS); /* Collapse current set, regardless of whether shaper works optimally with collapse or not. */
/* Loop over domain of command velocities */
for (sfact=-1.0, ext_min=DBL_MAX, ext_travel=DBL_MAX, uCmd_minext=uc; sfact<=1.0; sfact+=AS_UAUTO_INCREMENT) {
assignSet(&aSet,Setp); /* create work copy */
uCmd=sfact*umaxS;
sts |= mergeSet(&aSet, shp, (uCmd-uc - aSet.extSum/omega), amaxS, omega, 0, 0);
sts |= mergeSet(&dSet, shp, uCmd, amaxS, omega, 0, 0); /* will be reflected, shifted and inverted, which is why -uCmd is not used */
if (!sts && uCmd!=0.0) { /* If no error from mergeSet, and non-zero uCmd - continue on. */
/* Calculate the distance that will be travelled during acceleration and deceleration. */
Sxad = delta_x(&aSet, uc, 1.0/omega); /* distance travelled during acc. */
Sxad += delta_x(&dSet, 0.0, 1.0/omega); /* add distance travelled during dec. Note that 0.0 is used as uc. */
if (aSet.N == 0) /* No phasors allocated for the acceleration (u already equals uCmd) */
ext_a=0.0;
else
ext_a=-(aSet.ph[aSet.N - 1].phiEnd); /* phase duration of acc=(end of last phasor) */
/* Determine phase location for deceleration = -omega*time_for_dec */
phi_d=-(omega*(SxDiff-Sxad)/uCmd + ext_a);
/* Determine total travel phase extension */
if ((phi_d <= 0.0) && (overlap || -phi_d >= ext_a)) { /* Will only tolerate negative (or zero) phase (i.e. positive time), and deceleration start after acc if not overlap */
if (dSet.N > 0)
ext_travel=-(phi_d + dSet.ph[dSet.N - 1].phiEnd); /* total travel phase extension = offset_dec + ext_dec */
else /* Empty set needs special treatment. Situation should not occur, however.. */
ext_travel=-phi_d;
if (fabs(ext_travel-ext_match) < ext_min) { /* Found new minimum extension */
ext_min=fabs(ext_travel-ext_match);
uCmd_minext=uCmd;
}
}
}
else /* Reset error from mergeSet */
sts=0;
AS_emptySet(&dSet);
AS_emptySet(&aSet);
}
/* Check that a minimum positive extension was actually found before using uCmd_minext. */
if (ext_min != DBL_MAX) {
/* Use uCmd that yields min time */
uCmd=uCmd_minext;
assignSet(&aSet,Setp);
sts=0;
sts |= mergeSet(&aSet, shp, (uCmd-uc - aSet.extSum/omega), amaxS, omega, 0, verbose);
sts |= mergeSet(&dSet, shp, uCmd, amaxS, omega, 0, verbose); /* will be reflected, shifted and inverted, which is why -uCmd is not used */
/* Calculate the distance that will be travelled during acceleration and deceleration. */
Sxad = delta_x(&aSet, uc, 1.0/omega); /* distance travelled during acc. */
Sxad += delta_x(&dSet, 0.0, 1.0/omega); /* add distance travelled during dec. Note that 0.0 is used as uc. */
if (aSet.N == 0) /* No phasors allocated for the acceleration (u already equals uCmd) */
ext_a=0.0;
else
ext_a=-(aSet.ph[aSet.N - 1].phiEnd); /* phase duration of acc=(end of last phasor) */
/* Determine phase location for deceleration = -omega*time_for_dec */
phi_d=-(omega*(SxDiff-Sxad)/uCmd+ext_a);
/* Allocate phasors to final set. Acc part and Dec part. Collect status */
reflectShiftAndInvertSet(&dSet, phi_d); /* Create the deceleration part by reflecting, shifting and inverting the phasors */
assignSet(Setp,&aSet); /* Replace Set with the acceleration part */
AS_emptySet(&aSet); /* get rid of aSet */
appendSet(Setp, &dSet); /* Append the deceleration part */
AS_emptySet(&dSet); /* get rid of dSet */
qsort(Setp->ph, Setp->N, sizeof(AS_phasor), AS_cmpPhasors);
cleanSet(Setp);
/* Check for unallowed acc */
if (supAmax(Setp, shp->AMax*amaxH)) {
if (AS_SHP_NEG) { /* Negative shaper - redo with non-negative (dpulse or ddpulse) */
*astsp |= AS_AUTO_REMADE_SHP;
if (AS_SHP_ROBUST)
sts |= makeSet_auto(Setp, AS_SetupIST(AS_DDPULSE), astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, overlap);
else
sts |= makeSet_auto(Setp, AS_SetupIST(AS_DPULSE), astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, overlap);
}
else if (overlap) { /* Overlap - redo without overlap */
*astsp |= AS_AUTO_REMADE_OVERLAP;
sts |= makeSet_auto(Setp, shp, astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, 0);
}
else {/* No more tricks in the bag.. Give up. */
*astsp |= AS_AUTO_AMAX_FAILURE;
sts |= mergeSet(Setp, shp, (0.0-uc - Setp->extSum/omega), amaxS, omega, 0, verbose);
if (verbose >= AS_VERB_ERR)
printf("\nAntiSway/makeSet_auto: Could not make set without exceeding amax. Will set zero command velocity. amaxH=%f\n",shp->AMax*amaxH);
}
}
/* Check for too low velocities */
else if (subUmin(Setp, uc, 1.0/omega, umin)) { /* umin never exceeded */
if (ext_match != 0.0) {/* if matching a given ext, redo without matching */
*astsp |= AS_AUTO_REMADE_MATCH;
sts |= makeSet_auto(Setp, shp, astsp, SxDiff, 0.0, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, overlap);
}
else {
*astsp |= AS_AUTO_UMIN_FAILURE;
sts |= mergeSet(Setp, shp, (0.0-uc - Setp->extSum/omega), amaxS, omega, 0, verbose);
if (verbose >= AS_VERB_ERR)
printf("\nAntiSway/makeSet_auto: Could not make set with uR > umin. Will set zero command velocity. umin=%f\n",umin);
}
}
/* Check for too high velocities */
else if (supUmax(Setp, uc, 1.0/omega, umaxH)) {
if (overlap) {/* Overlap - redo without overlap */
*astsp |= AS_AUTO_REMADE_OVERLAP;
sts |= makeSet_auto(Setp, shp, astsp, SxDiff, ext_match, uc, amaxH, amaxS, umaxH, umaxS, umin, omega, verbose, 0);
}
else /* Resign.. use the set with umax violation (will be cut down in exec) */
*astsp |= AS_AUTO_UMAX_FAILURE;
}
}
/* No minimum positive extension found.. */
else {
*astsp |= AS_AUTO_POSEXT_FAILURE;
sts |= mergeSet(Setp, shp, (0.0-uc - Setp->extSum/omega), amaxS, omega, 0, verbose);
if (verbose >= AS_VERB_ERR)
printf("\nAntiSway/makeSet_auto: Could not find a minimum positive extension. Will set zero command velocity.\n");
}
return sts;
}
/**
* Add all elements of other to self (obviously, without creating duplicate elements).
*
* My notes: If 'self' is unsorted, if both sets are empty, or if only 'other' is empty,
* do nothing ('self' unioned with an empty set is just 'self.'). Additionally, if just
* 'self' is empty, use assignSet and return (quicker). Similar to subtractFromSet, checks
* if index used for corresponding set (selfIndex to 'self' and otherIndex to 'other') is
* equal to their corresponding lengths before the union. If the index is not equal to the
* length, there are unique members in either set that either 1) haven't been stored in 'self'
* or 2) haven't been stored in the correct index in 'self.' The two for loops beneath the
* while loop resolves this for both sets. Finally, update the new length of 'self.'
*
* param self: set to hold the union of 'self' and 'other.'
* param other: set to compare with 'self' for the union.
* return: N/A
*/
void unionInSet(Set* self, const Set* other)
{
if ((self->len == 0 && other->len == 0) || /* 'self' already has union for first two lines, or 'self' doesn't obey design invariant properties */
(self->len != 0 && other->len == 0) ||
(!isSorted(self)))
{
return;
}
if (self->len == 0 && other->len != 0) { /* more convenient to use assignSet for an empty 'self' and nonempty 'other' */
assignSet(self, other);
return;
}
int selfIndex = 0;
int otherIndex = 0;
int unionIndex = 0;
int unionArr[100000]; /* initialize large temp array to hold union members */
while (selfIndex < self->len && otherIndex < other->len) {
if (self->elements[selfIndex] == other->elements[otherIndex]) { /* element found in both sets, so add it */
unionArr[unionIndex] = self->elements[selfIndex];
unionIndex++;
selfIndex++;
otherIndex++;
}
else if (self->elements[selfIndex] > other->elements[otherIndex]) {
unionArr[unionIndex] = other->elements[otherIndex];
unionIndex++;
otherIndex++;
}
else if (self->elements[selfIndex] < other->elements[otherIndex]) {
unionArr[unionIndex] = self->elements[selfIndex];
unionIndex++;
selfIndex++;
}
}
if (self->len != selfIndex) { /* accounts for unchecked members in 'self' set */
for (int index = selfIndex; index < self->len; index++) {
unionArr[unionIndex] = self->elements[index];
unionIndex++;
}
}
if (other->len != otherIndex) { /* accounts for unchecked members in 'other' set */
for (int index = otherIndex; index < other->len; index++) {
unionArr[unionIndex] = other->elements[index];
unionIndex++;
}
}
if (self->len < unionIndex) { /* the unioned set exceeds the old capacity of 'self' */
self->elements = (int*) realloc(self->elements, unionIndex * sizeof(int)); /* reallocate memory so 'self' can hold unioned set */
}
for (int index = 0; index < unionIndex; index++) { /* write all unioned members back to 'self' */
self->elements[index] = unionArr[index];
}
self->len = unionIndex; /* update length of 'self' set. */
}
void algebraicTests(void) {
Set empty;
Set universal;
int i;
Set s;
Set t;
Set u;
Set v;
Set w;
createEmptySet(&empty);
createEmptySet(&universal);
for (i = 0; i < maximum_set_size; i += 1) {
insertSet(&universal, i);
}
createEmptySet(&s);
createEmptySet(&t);
createEmptySet(&u);
createEmptySet(&v);
createEmptySet(&w);
for (i = 0; i < number_of_tests; i += 1) {
randomSet(&u);
randomSet(&v);
randomSet(&w);
/* u + v == v + u */
assignSet(&s, &u);
unionInSet(&s, &v);
assignSet(&t, &v);
unionInSet(&t, &u);
assert(isEqualToSet(&s, &t));
/* u + (v + w) == (u + v) + w */
assignSet(&t, &v);
unionInSet(&t, &w);
assignSet(&s, &u);
unionInSet(&s, &t);
assignSet(&t, &u);
unionInSet(&t, &v);
unionInSet(&t, &w);
assert(isEqualToSet(&s, &t));
/* u * v == v * u */
assignSet(&s, &u);
intersectFromSet(&s, &v);
assignSet(&t, &v);
intersectFromSet(&t, &u);
assert(isEqualToSet(&s, &t));
/* u * (v * w) == (u * v) * w */
assignSet(&t, &v);
intersectFromSet(&t, &w);
assignSet(&s, &u);
intersectFromSet(&s, &t);
assignSet(&t, &u);
intersectFromSet(&t, &v);
intersectFromSet(&t, &w);
assert(isEqualToSet(&s, &t));
/* u - v == u - (u * v) */
assignSet(&s, &u);
intersectFromSet(&s, &v);
assignSet(&t, &u);
subtractFromSet(&t, &s);
assignSet(&s, &u);
subtractFromSet(&s, &v);
assert(isEqualToSet(&s, &t));
/* additional tests, not implemented
assert(w * (u - v) == w * u - w * v);
assert(u * (v + w) == (u * v) + (u * w));
assert(universal - (u * v) == (universal - u) + (universal - v));
assert(universal - (u + v) == (universal - u) * (universal - v));
*/
}
printf("The algebraic tests have been passed\n");
destroySet(&empty);
destroySet(&universal);
destroySet(&s);
destroySet(&t);
destroySet(&u);
destroySet(&v);
destroySet(&w);
}
void specialCaseTests(void) {
Set empty;
Set universal;
int i;
Set s;
Set r;
createEmptySet(&empty);
createEmptySet(&universal);
createEmptySet(&r);
for (i = 0; i < maximum_set_size; i += 1) {
insertSet(&universal, i);
}
checkCase(&subtractFromSet, &universal, &universal, &empty);
checkCase(&unionInSet, &universal, &universal, &universal);
checkCase(&intersectFromSet, &universal, &universal, &universal);
checkCase(&intersectFromSet, &universal, &empty, &empty);
checkCase(&intersectFromSet, &empty, &universal, &empty);
checkCase(&unionInSet, &universal, &empty, &universal);
checkCase(&unionInSet, &empty, &universal, &universal);
checkCase(&unionInSet, &empty, &empty, &empty);
checkCase(&subtractFromSet, &empty, &empty, &empty);
checkCase(&intersectFromSet, &empty, &empty, &empty);
createEmptySet(&s);
assert(isEmptySet(&s));
for (i = 0; i < 10; i += 1) {
insertSet(&s, i);
}
assert(s.len == 10);
for (i = 0; i < 10; i += 1) {
assert(isMemberSet(&s, i));
}
for (i = 0; i < 10; i += 1) {
removeSet(&s, i);
removeSet(&s, i);
assert(s.len == 9 - i);
}
assert(isEmptySet(&s));
for (i = 0; i < number_of_tests; i += 1) {
randomSet(&s);
assert(isSubsetOf(&empty, &s));
assert(!isSubsetOf(&s, &empty));
assert(isSubsetOf(&s, &universal));
assert(!isSubsetOf(&universal, &s));
checkCase(&intersectFromSet, &empty, &s, &empty);
checkCase(&intersectFromSet, &s, &empty, &empty);
checkCase(&intersectFromSet, &universal, &s, &s);
checkCase(&intersectFromSet, &s, &universal, &s);
checkCase(&unionInSet, &universal, &s, &universal);
checkCase(&unionInSet, &s, &universal, &universal);
checkCase(&subtractFromSet, &s, &empty, &s);
assignSet(&r, &universal);
subtractFromSet(&r, &s); // r = u - s;
checkCase(&subtractFromSet, &universal, &r, &s); // (u - (u - s) == s)
checkCase(&unionInSet, &s, &r, &universal); // s + (u - s) == u
checkCase(&unionInSet, &r, &s, &universal); // (u - s) + s == u
}
printf("The special case tests have been passed\n");
destroySet(&empty);
destroySet(&universal);
destroySet(&s);
destroySet(&r);
}
TEST(Test07, AlgebraicTests)
{
{
Set empty;
Set universal;
Set r;
Set s;
Set t;
Set u;
Set v;
Set w;
int i;
createEmptySet(&empty);
createEmptySet(&universal);
for (i = 0; i < MAX_SET_SIZE; i++)
{
insertSet(&universal, i);
}
createEmptySet(&r);
createEmptySet(&s);
createEmptySet(&t);
createEmptySet(&u);
createEmptySet(&v);
createEmptySet(&w);
ASSERT_FALSE(isEqualToSet(&empty, &universal));
for (i = 0; i < NUM_TESTS; i++)
{
randomSet(&u);
randomSet(&v);
randomSet(&w);
/* w * (u - v) == w * u - w * v */
assignSet(&s, &u);
subtractFromSet(&s, &v);
assignSet(&r, &w);
intersectFromSet(&r, &s);
assignSet(&t, &w);
intersectFromSet(&t, &v);
assignSet(&s, &w);
intersectFromSet(&s, &u);
subtractFromSet(&s, &t);
ASSERT_TRUE(isEqualToSet(&r, &s));
/* u * (v + w) == (u * v) + (u * w) */
assignSet(&s, &v);
unionInSet(&s, &w);
assignSet(&r, &u);
intersectFromSet(&r, &s);
assignSet(&t, &u);
intersectFromSet(&t, &w);
assignSet(&s, &u);
intersectFromSet(&s, &v);
unionInSet(&s, &t);
ASSERT_TRUE(isEqualToSet(&r, &s));
/* universal - (u * v) == (universal - u) + (universal - v) */
assignSet(&s, &u);
intersectFromSet(&s, &v);
assignSet(&r, &universal);
subtractFromSet(&r, &s);
assignSet(&t, &universal);
subtractFromSet(&t, &v);
assignSet(&s, &universal);
subtractFromSet(&s, &u);
unionInSet(&s, &t);
ASSERT_TRUE(isEqualToSet(&r, &s));
/* universal - (u + v) == (universal - u) * (universal - v) */
assignSet(&s, &u);
unionInSet(&s, &v);
assignSet(&r, &universal);
subtractFromSet(&r, &s);
assignSet(&t, &universal);
subtractFromSet(&t, &v);
assignSet(&s, &universal);
subtractFromSet(&s, &u);
intersectFromSet(&s, &t);
ASSERT_TRUE(isEqualToSet(&r, &s));
}
destroySet(&empty);
destroySet(&universal);
destroySet(&r);
destroySet(&s);
destroySet(&t);
destroySet(&u);
destroySet(&v);
destroySet(&w);
}
}
TEST(Test05, SpecialCaseTests)
{
{
Set empty;
Set universal;
Set s;
Set r;
int i;
createEmptySet(&empty);
createEmptySet(&universal);
createEmptySet(&r);
for (i = 0; i < MAX_SET_SIZE; i++)
{
insertSet(&universal, i);
}
ASSERT_TRUE(checkCaseNew(&subtractFromSet, &universal, &universal, &empty));
ASSERT_TRUE(checkCaseNew(&unionInSet, &universal, &universal, &universal));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &universal, &universal, &universal));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &universal, &empty, &empty));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &empty, &universal, &empty));
ASSERT_TRUE(checkCaseNew(&unionInSet, &universal, &empty, &universal));
ASSERT_TRUE(checkCaseNew(&unionInSet, &empty, &universal, &universal));
ASSERT_TRUE(checkCaseNew(&unionInSet, &empty, &empty, &empty));
ASSERT_TRUE(checkCaseNew(&subtractFromSet, &empty, &empty, &empty));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &empty, &empty, &empty));
createEmptySet(&s);
ASSERT_TRUE(isEmptySet(&s));
for (i = 0; i < 10; i++)
{
insertSet(&s, i);
}
ASSERT_TRUE(s.len == 10);
for (i = 0; i < 10; i++)
{
ASSERT_TRUE(isMemberSet(&s, i));
}
for (i = 0; i < 10; i++)
{
removeSet(&s, i);
removeSet(&s, i);
ASSERT_TRUE(s.len == 9 - i);
}
ASSERT_TRUE(isEmptySet(&s));
for (i = 0; i < NUM_TESTS; i++)
{
randomSet(&s);
ASSERT_TRUE(isSubsetOf(&empty, &s));
ASSERT_FALSE(isSubsetOf(&s, &empty));
ASSERT_TRUE(isSubsetOf(&s, &universal));
ASSERT_FALSE(isSubsetOf(&universal, &s));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &empty, &s, &empty));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &s, &empty, &empty));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &universal, &s, &s));
ASSERT_TRUE(checkCaseNew(&intersectFromSet, &s, &universal, &s));
ASSERT_TRUE(checkCaseNew(&unionInSet, &universal, &s, &universal));
ASSERT_TRUE(checkCaseNew(&unionInSet, &s, &universal, &universal));
ASSERT_TRUE(checkCaseNew(&subtractFromSet, &s, &empty, &s));
assignSet(&r, &universal);
subtractFromSet(&r, &s);
ASSERT_TRUE(checkCaseNew(&subtractFromSet, &universal, &r, &s));
ASSERT_TRUE(checkCaseNew(&unionInSet, &s, &r, &universal));
ASSERT_TRUE(checkCaseNew(&unionInSet, &r, &s, &universal));
}
destroySet(&empty);
destroySet(&universal);
destroySet(&s);
destroySet(&r);
}
}
