void IIRFilter::getFrequencyResponse(int nFrequencies, const float* frequency, float* magResponse, float* phaseResponse)
{
// Evaluate the z-transform of the filter at the given normalized frequencies from 0 to 1. (One
// corresponds to the Nyquist frequency.)
//
// The z-tranform of the filter is
//
// H(z) = sum(b[k]*z^(-k), k, 0, M) / sum(a[k]*z^(-k), k, 0, N);
//
// The desired frequency response is H(exp(j*omega)), where omega is in [0, 1).
//
// Let P(x) = sum(c[k]*x^k, k, 0, P) be a polynomial of order P. Then each of the sums in H(z)
// is equivalent to evaluating a polynomial at the point 1/z.
for (int k = 0; k < nFrequencies; ++k) {
// zRecip = 1/z = exp(-j*frequency)
double omega = -M_PI * frequency[k];
std::complex<double> zRecip = std::complex<double>(cos(omega), sin(omega));
std::complex<double> numerator = evaluatePolynomial(m_feedforward->Elements(), zRecip, m_feedforward->Length() - 1);
std::complex<double> denominator = evaluatePolynomial(m_feedback->Elements(), zRecip, m_feedback->Length() - 1);
// Strangely enough, using complex division:
// e.g. Complex response = numerator / denominator;
// fails on our test machines, yielding infinities and NaNs, so we do
// things the long way here.
double n = norm(denominator);
double r = (real(numerator)*real(denominator) + imag(numerator)*imag(denominator)) / n;
double i = (imag(numerator)*real(denominator) - real(numerator)*imag(denominator)) / n;
std::complex<double> response = std::complex<double>(r, i);
magResponse[k] = static_cast<float>(abs(response));
phaseResponse[k] = static_cast<float>(atan2(imag(response), real(response)));
}
}
int test_evaluatePolynomial() {
printf("--- Test evaluatePolynomial ---\n");
error_code err = NONE;
Polynomial *p1 = parsePolynomial("0 0", &err),
*p2 = parsePolynomial("1 1 1 0", &err);
if(!p1 || !p2) {
printError(stdout, &err, NULL);
freePolynomial(p1);
freePolynomial(p2);
return 0;
}
double a = evaluatePolynomial(p1, 0.2, &err);
double b = evaluatePolynomial(p2, 0.2, &err);
if(err != NONE) {
printError(stdout, &err, NULL);
freePolynomial(p1);
freePolynomial(p2);
return 0;
}
if(a != 0. || b != 1.2) {
printf(" Wrong evaluation.\n");
freePolynomial(p1);
freePolynomial(p2);
return 0;
}
freePolynomial(p1);
freePolynomial(p2);
return 1;
}
//==============================================================================
void Spline::evaluateDerivative(
double _t, int _derivative, Eigen::VectorXd& _tangentVector) const
{
if (mSegments.empty())
throw std::logic_error("Unable to evaluate empty trajectory.");
if (_derivative < 1)
throw std::logic_error("Derivative must be positive.");
const auto targetSegmentInfo = getSegmentForTime(_t);
const auto& targetSegment = mSegments[targetSegmentInfo.first];
const auto evaluationTime = _t - targetSegmentInfo.second;
// Return zero for higher-order derivatives.
if (_derivative < targetSegment.mCoefficients.cols())
{
// TODO: We should transform this into the body frame using the adjoint
// transformation.
_tangentVector = evaluatePolynomial(
targetSegment.mCoefficients, evaluationTime, _derivative);
}
else
{
_tangentVector.resize(mStateSpace->getDimension());
_tangentVector.setZero();
}
}
std::vector< share_t > share(const mpz_class& secret, unsigned int t, unsigned int n, const mpz_class& p, gmp_randclass& randomGenerator, unsigned int security)
{
std::vector<mpz_class> coefficients = getRandomPolynomial(t - 1, p, randomGenerator, security);
coefficients[0] = secret;
std::vector< share_t > shares;
shares.resize(n);
for (size_t i = 0; i < shares.size(); ++i)
{
shares[i].first = mpz_class(i + 1);
shares[i].second = evaluatePolynomial(i + 1, coefficients, p);
}
return shares;
}
//==============================================================================
void Spline::evaluate(double _t, statespace::StateSpace::State* _out) const
{
if (mSegments.empty())
throw std::logic_error("Unable to evaluate empty trajectory.");
const auto targetSegmentInfo = getSegmentForTime(_t);
const auto& targetSegment = mSegments[targetSegmentInfo.first];
mStateSpace->copyState(targetSegment.mStartState, _out);
const auto evaluationTime = _t - targetSegmentInfo.second;
const auto tangentVector
= evaluatePolynomial(targetSegment.mCoefficients, evaluationTime, 0);
auto relativeState = mStateSpace->createState();
mStateSpace->expMap(tangentVector, relativeState);
mStateSpace->compose(_out, relativeState);
}
void
paillier_keygen( int modulusbits, int decryptServers, int thresholdServers,
paillier_pubkey_t** pub,
paillier_partialkey_t*** partKeys,
paillier_get_rand_t get_rand )
{
mpz_t p1;
mpz_t q1;
mpz_t p;
mpz_t q;
mpz_t m;
mpz_t nm;
mpz_t nSquare;
mpz_t mInversToN;
mpz_t d;
mpz_t r;
mpz_t gcdRN;
mpz_t vExp;
mpz_t * a;
mpz_t * shares;
mpz_t * viarray;
gmp_randstate_t rand;
/* allocate the new key structures */
*pub = (paillier_pubkey_t*) malloc(sizeof(paillier_pubkey_t));
*partKeys = (paillier_partialkey_t**) malloc(sizeof(paillier_partialkey_t*) * decryptServers);
/* initialize our integers */
mpz_init((*pub)->n);
mpz_init((*pub)->n_squared);
mpz_init((*pub)->n_plusone);
mpz_init((*pub)->delta);
mpz_init((*pub)->combineSharesConstant);
mpz_init((*pub)->v);
mpz_init(m);
mpz_init(nm);
mpz_init(nSquare);
mpz_init(mInversToN);
mpz_init(d);
mpz_init(r);
mpz_init(gcdRN);
mpz_init(vExp);
/* pick random (modulusbits/2)-bit primes p and q */
init_rand(rand, get_rand, modulusbits / 8 + 1);
do
{
genSafePrimes(&p1,&p,modulusbits,rand);
do
genSafePrimes(&q1,&q,modulusbits,rand);
while( mpz_cmp(p,q)==0 || mpz_cmp(p,q1)==0 || mpz_cmp(q,p1)==0 || mpz_cmp(q1,p1)==0 );
/* compute the public modulus n = p q */
mpz_mul((*pub)->n, p, q);
mpz_mul(m, p1, q1);
} while( !mpz_tstbit((*pub)->n, modulusbits - 1) );
(*pub)->bits = modulusbits;
(*pub)->decryptServers = decryptServers;
(*pub)->threshold = thresholdServers;
(*pub)->complete();
// We decide on some s>0, thus the plaintext space will be Zn^s
// for now we set s=1
mpz_mul(nm, (*pub)->n, m);
mpz_mul(nSquare, (*pub)->n, (*pub)->n);
// next d need to be chosen such that
// d=0 mod m and d=1 mod n, using Chinese remainder thm
// we can find d using Chinese remainder thm
// note that $d=(m. (m^-1 mod n))$
int errorCode = mpz_invert(mInversToN,m,(*pub)->n);
if (errorCode == 0)
{
throw std::runtime_error("Inverse of m mod n not found!");
}
mpz_mul(d,m,mInversToN);
//a[0] is equal to d
//a[i] is the random number used for generating the polynomial
//between 0... n^s*m-1, for 0 < i < thresholdServers
a = (mpz_t*) malloc(sizeof(mpz_t) * thresholdServers);
mpz_init(a[0]);
mpz_set(a[0], d);
for (int i = 1; i < thresholdServers; ++i) {
mpz_init(a[i]);
mpz_urandomm(a[i], rand, nm);
}
//We need to generate v
//Although v needs to be the generator of the squares in Z^*_{n^2}
//I will use a heuristic which gives a generator with high prob.
//get a random element r such that gcd(r,nSquare) is one
//set v=r*r mod nSquare. This heuristic is used in the Victor Shoup
//threshold signature paper.
do
{
mpz_urandomb(r, rand, 4*modulusbits);
mpz_gcd(gcdRN, r, (*pub)->n);
} while( !mpz_cmp_ui(gcdRN, 1)==0 );
// we can now set v to r*r mod nSquare
mpz_powm_ui((*pub)->v, r, 2, nSquare);
//This array holds the resulting keys
shares = (mpz_t*) malloc(sizeof(mpz_t) * decryptServers);
viarray = (mpz_t*) malloc(sizeof(mpz_t) * decryptServers);
for (int i = 0; i < decryptServers; ++i) {
mpz_init(shares[i]);
mpz_init(viarray[i]);
// The secret share of the i'th authority will be s_i=f(i) for 1<=i<=l
paillier_polynomial_point_t *polynPoint = evaluatePolynomial(a, thresholdServers, i+1, nm);
mpz_set(shares[i], polynPoint->p);
paillier_freepolynomialpoint(polynPoint);
//for each decryption server a verication key v_i=v^(delta*s_i) mod n^(s+1)
mpz_mul(vExp, (*pub)->delta, shares[i]);
mpz_powm(viarray[i], (*pub)->v, vExp, nSquare);
}
/* Set key structures */
(*pub)->verificationKeys = (paillier_verificationkey_t**) malloc(sizeof(paillier_verificationkey_t) * decryptServers);
for (int i = 0; i < decryptServers; ++i) {
int id = i+1;
paillier_verificationkey_t* verKey = (paillier_verificationkey_t*) malloc(sizeof(paillier_verificationkey_t));
mpz_init(verKey->v);
verKey->id=id;
mpz_set(verKey->v, viarray[i]);
(*pub)->verificationKeys[i]=verKey;
paillier_partialkey_t* share = (paillier_partialkey_t*) malloc(sizeof(paillier_partialkey_t));
mpz_init(share->s);
share->id=id;
mpz_set(share->s, shares[i]);
(*partKeys)[i]=share;
}
/* clear temporary integers and randstate */
mpz_clear(p);
mpz_clear(q);
mpz_clear(p1);
mpz_clear(q1);
mpz_clear(m);
mpz_clear(nm);
mpz_clear(nSquare);
mpz_clear(mInversToN);
mpz_clear(d);
mpz_clear(r);
mpz_clear(gcdRN);
mpz_clear(vExp);
gmp_randclear(rand);
for (int i = 0; i < thresholdServers; ++i) {
mpz_clear(a[i]);
}
free(a);
for (int i = 0; i < decryptServers; ++i) {
mpz_clear(shares[i]);
mpz_clear(viarray[i]);
}
free(shares);
free(viarray);
}