VoltageBehindReactanceEMT.cpp 12.5 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
/** Voltage behind reactance (EMT)
 *
 * @author Markus Mirz <mmirz@eonerc.rwth-aachen.de>
 * @copyright 2017, Institute for Automation of Complex Power Systems, EONERC
 * @license GNU General Public License (version 3)
 *
 * DPsim
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *********************************************************************************/

23
#include "VoltageBehindReactanceEMT.h"
Markus Mirz's avatar
Markus Mirz committed
24
#include "../IntegrationMethod.h"
25
26
27

using namespace DPsim;

28
VoltageBehindReactanceEMT::VoltageBehindReactanceEMT(String name, Int node1, Int node2, Int node3,
Markus Mirz's avatar
Markus Mirz committed
29
	Real nomPower, Real nomVolt, Real nomFreq, Int poleNumber, Real nomFieldCur,
30
	Real Rs, Real Ll, Real Lmd, Real Lmd0, Real Lmq, Real Lmq0,
31
32
	Real Rfd, Real Llfd, Real Rkd, Real Llkd,
	Real Rkq1, Real Llkq1, Real Rkq2, Real Llkq2,
33
	Real inertia, bool logActive)
34
35
36
	: SynchGenBase(name, node1, node2, node3, nomPower, nomVolt, nomFreq, poleNumber, nomFieldCur,
		Rs, Ll, Lmd, Lmd0, Lmq, Lmq0, Rfd, Llfd, Rkd, Llkd, Rkq1, Llkq1, Rkq2, Llkq2,
		inertia, logActive) {
Steffen Vogel's avatar
Steffen Vogel committed
37

38
}
39
40
41
42
43
44
45

VoltageBehindReactanceEMT::~VoltageBehindReactanceEMT() {
	if (mLogActive) {
		delete mLog;
	}
}

46
47
48
49
50
void VoltageBehindReactanceEMT::AddExciter(Real Ta, Real Ka, Real Te, Real Ke, Real Tf, Real Kf, Real Tr, Real Lad, Real Rfd) {
	mExciter = Exciter(Ta, Ka, Te, Ke, Tf, Kf, Tr, Lad, Rfd, mVfd);
	WithExciter = true;
}

51
52
53
54
55
void VoltageBehindReactanceEMT::AddGovernor(Real Ta, Real Tb, Real Tc, Real Fa, Real Fb, Real Fc, Real K, Real Tsr, Real Tsm, Real Tm_init){
	mTurbineGovernor = TurbineGovernor(Ta, Tb, Tc, Fa, Fb, Fc, K, Tsr, Tsm, Tm_init);
	WithTurbineGovernor = true;
}

56

57

58
59
void VoltageBehindReactanceEMT::init(Real om, Real dt,
	Real initActivePower, Real initReactivePower, Real initTerminalVolt, Real initVoltAngle, Real initFieldVoltage, Real initMechPower) {
60

61
62
63
64
65
	mResistanceMat = Matrix::Zero(3, 3);
	mResistanceMat <<
		mRs, 0, 0,
		0, mRs, 0,
		0, 0, mRs;
66

67
68
69
70
	R_load <<
		1037.8378 / mBase_Z, 0, 0,
		0, 1037.8378 / mBase_Z, 0,
		0, 0, 1037.8378 / mBase_Z;
Viviane Sapucaia's avatar
Viviane Sapucaia committed
71

72
	//Dynamic mutual inductances
73

Viviane Sapucaia's avatar
Viviane Sapucaia committed
74
	mDLmd = 1. / (1. / mLmd + 1. / mLlfd + 1. / mLlkd);
75
	if (mNumDampingWindings == 2)
Viviane Sapucaia's avatar
Viviane Sapucaia committed
76
	{
Viviane Sapucaia's avatar
Viviane Sapucaia committed
77
		mDLmq = 1. / (1. / mLmq + 1. / mLlkq1 + 1. / mLlkq2);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
78
79
80
81
82
83
84
85
86
87
88
89

		A_flux <<
			-mRkq1 / mLlkq1*(1 - mDLmq / mLlkq1), mRkq1 / mLlkq1*(mDLmq / mLlkq2), 0, 0,
			mRkq2 / mLlkq2*(mDLmq / mLlkq1), -mRkq2 / mLlkq2*(1 - mDLmq / mLlkq2), 0, 0,
			0, 0, -mRfd / mLlfd*(1 - mDLmd / mLlfd), mRfd / mLlfd*(mDLmd / mLlkd),
			0, 0, mRkd / mLlkd*(mDLmd / mLlfd), -mRkd / mLlkd*(1 - mDLmd / mLlkd);
		B_flux <<
			mRkq1*mDLmq / mLlkq1, 0,
			mRkq2*mDLmq / mLlkq2, 0,
			0, mRfd*mDLmd / mLlfd,
			0, mRkd*mDLmd / mLlkd;
	}
90

Viviane Sapucaia's avatar
Viviane Sapucaia committed
91
	else
92
	{
Viviane Sapucaia's avatar
Viviane Sapucaia committed
93
		mDLmq = 1. / (1. / mLmq + 1. / mLlkq1);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
94

Markus Mirz's avatar
Markus Mirz committed
95
96
97
98
99
100
		A_flux = Matrix::Zero(3, 3);
		B_flux = Matrix::Zero(3, 2);
		C_flux = Matrix::Zero(3, 1);
		mRotorFlux = Matrix::Zero(3, 1);
		mDqStatorCurrents = Matrix::Zero(2, 1);
		mDqStatorCurrents_hist = Matrix::Zero(2, 1);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
101
102
103
104
105
106
107
108

		A_flux <<
			-mRkq1 / mLlkq1*(1 - mDLmq / mLlkq1), 0, 0,
			0, -mRfd / mLlfd*(1 - mDLmd / mLlfd), mRfd / mLlfd*(mDLmd / mLlkd),
			0, mRkd / mLlkd*(mDLmd / mLlfd), -mRkd / mLlkd*(1 - mDLmd / mLlkd);
		B_flux <<
			mRkq1*mDLmq / mLlkq1, 0,
			0, mRfd*mDLmd / mLlfd,
109
			0, mRkd*mDLmd / mLlkd;
Viviane Sapucaia's avatar
Viviane Sapucaia committed
110
111
	}

Viviane Sapucaia's avatar
Viviane Sapucaia committed
112
113
114
	mLa = (mDLmq + mDLmd) / 3.;
	mLb = (mDLmd - mDLmq) / 3.;

115
	// steady state per unit initial value
116
	initStatesInPerUnit(initActivePower, initReactivePower, initTerminalVolt, initVoltAngle, initFieldVoltage, initMechPower);
117

118

119
120
121
122
123
	// Correcting variables
	mThetaMech = mThetaMech + PI/2;
	mMechTorque = -mMechTorque;
	mIq = -mIq;
	mId = -mId;
124
125

	// #### VBR Model Dynamic variables #######################################
126
	
127
128
	mDPsid = mDLmd*(mPsifd / mLlfd) + mDLmd*(mPsikd / mLlkd);

129
	if (mNumDampingWindings == 2) {
130
		mDPsiq = mDLmq*(mPsikq1 / mLlkq1) + mDLmq*(mPsikq2 / mLlkq2);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
131
132
		mDVq = mOmMech*mDPsid + mDLmq*mRkq1*(mDPsiq - mPsikq1) / (mLlkq1*mLlkq1) +
			mDLmq*mRkq2*(mDPsiq - mPsikq2) / (mLlkq2*mLlkq2) + (mRkq1 / (mLlkq1*mLlkq1) + mRkq2 / (mLlkq2*mLlkq2))*mDLmq*mDLmq*mIq;
133
		mPsimq = mDLmq*(mPsikq1 / mLlkq1 + mPsikq2 / mLlkq2 + mIq);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
134
135
	}
	else {
136
		mDPsiq = mDLmq*(mPsikq1 / mLlkq1);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
137
		mDVq = mOmMech*mDPsid + mDLmq*mRkq1*(mDPsiq - mPsikq1) / (mLlkq1*mLlkq1) + (mRkq1 / (mLlkq1*mLlkq1))*mDLmq*mDLmq*mIq;
138
		mPsimq = mDLmq*(mPsikq1 / mLlkq1 + mIq);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
139
	}
140

141
142
	mDVd = -mOmMech*mDPsiq + mDLmd*mRkd*(mDPsid - mPsikd) / (mLlkd*mLlkd) + (mDLmd / mLlfd)*mVfd +
		mDLmd*mRfd*(mDPsid - mPsifd) / (mLlfd*mLlfd) + (mRfd / (mLlfd*mLlfd) + mRkd / (mLlkd*mLlkd))*mDLmd*mDLmd*mId;
143
	mPsimd = mDLmd*(mPsifd / mLlfd + mPsikd / mLlkd + mId);
144
145
146
147

	mDVa = inverseParkTransform(mThetaMech, mDVq, mDVd, 0)(0);
	mDVb = inverseParkTransform(mThetaMech, mDVq, mDVd, 0)(1);
	mDVc = inverseParkTransform(mThetaMech, mDVq, mDVd, 0)(2);
148
	
149
150
151
152
153
	mDVabc <<
		mDVa,
		mDVb,
		mDVc;
	mDVabc_hist = mDVabc;
154

155
156
157
	mVa = inverseParkTransform(mThetaMech, mVq, mVd, mV0)(0);
	mVb = inverseParkTransform(mThetaMech, mVq, mVd, mV0)(1);
	mVc = inverseParkTransform(mThetaMech, mVq, mVd, mV0)(2);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
158

159
160
161
	mIa = inverseParkTransform(mThetaMech, mIq, mId, mI0)(0);
	mIb = inverseParkTransform(mThetaMech, mIq, mId, mI0)(1);
	mIc = inverseParkTransform(mThetaMech, mIq, mId, mI0)(2);
162
163
}

164

165
void VoltageBehindReactanceEMT::step(SystemModel& system, Real time) {
166

167
168
	stepInPerUnit(system.getOmega(), system.getTimeStep(), time, system.getNumMethod());

169
170
	R_load = system.getCurrentSystemMatrix().inverse() / mBase_Z;

171
172
173
174
175
176
177
178
179
180
	// Update current source accordingly
	if (mNode1 >= 0) {
		system.addRealToRightSideVector(mNode1, -mIa*mBase_i);
	}
	if (mNode2 >= 0) {
		system.addRealToRightSideVector(mNode2, -mIb*mBase_i);
	}
	if (mNode3 >= 0) {
		system.addRealToRightSideVector(mNode3, -mIc*mBase_i);
	}
181

182
	if (mLogActive) {
183
184
		Matrix logValues(getRotorFluxes().rows() + getDqStatorCurrents().rows() + 3, 1);
		logValues << getRotorFluxes(), getDqStatorCurrents(), getElectricalTorque(), getRotationalSpeed(), getRotorPosition();
185
186
		mLog->LogDataLine(time, logValues);
	}
187

188

189
190
}

191

192
void VoltageBehindReactanceEMT::stepInPerUnit(Real om, Real dt, Real time, NumericalMethod numMethod) {
193
194
195
196
197
198

	mIabc <<
		mIa,
		mIb,
		mIc;

199
	// Calculate mechanical variables with euler
200
201
202
203
204
	if (WithTurbineGovernor == true)
	{
		mMechTorque = -mTurbineGovernor.step(mOmMech, 1, 0.001, dt);

	}
205
	mElecTorque = (mPsimd*mIq - mPsimq*mId);
206
	mOmMech = mOmMech + dt * (1. / (2. * mH) * (mElecTorque - mMechTorque));
Viviane Sapucaia's avatar
Viviane Sapucaia committed
207
	mThetaMech = mThetaMech + dt * (mOmMech* mBase_OmMech);
Steffen Vogel's avatar
Steffen Vogel committed
208

209

210
	// Calculate Inductance matrix and its derivative
211
	CalculateLandpL();
Viviane Sapucaia's avatar
Viviane Sapucaia committed
212

213
	// Solve circuit - calculate stator currents
214
	if (numMethod == NumericalMethod::Trapezoidal_flux) {
215
		mIabc = Trapezoidal(mIabc, -mDInductanceMat.inverse()*(mResistanceMat + R_load + pmDInductanceMat), mDInductanceMat.inverse(), dt*mBase_OmElec, -mDVabc, -mDVabc_hist);
216
217
218
		
	}
		
219
220
	else if (numMethod == NumericalMethod::Euler) 
		mIabc = Euler(mIabc, -mDInductanceMat.inverse()*(mResistanceMat + R_load + pmDInductanceMat), mDInductanceMat.inverse(), dt*mBase_OmElec, -mDVabc);
221
222
223
224
225
226
	


	mIa_hist = mIa;
	mIb_hist = mIb;
	mIc_hist = mIc;
227

Viviane Sapucaia's avatar
Viviane Sapucaia committed
228
229
230
	mIa = mIabc(0);
	mIb = mIabc(1);
	mIc = mIabc(2);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
231
232
	Real mIq_hist = mIq;
	Real mId_hist = mId;
Viviane Sapucaia's avatar
Viviane Sapucaia committed
233
234
	mIq = parkTransform(mThetaMech, mIa, mIb, mIc)(0);
	mId = parkTransform(mThetaMech, mIa, mIb, mIc)(1);
Steffen Vogel's avatar
Steffen Vogel committed
235
	mI0 = parkTransform(mThetaMech, mIa, mIb, mIc)(2);
236

237
238
239
240
241
242
243
244
245
	if (WithExciter == true) {

		// dq-transform of interface voltage
		mVd = parkTransform(mThetaMech, mVa / mBase_v, mVb / mBase_v, mVc / mBase_v)(0);
		mVq = parkTransform(mThetaMech, mVa / mBase_v, mVb / mBase_v, mVc / mBase_v)(1);
		mV0 = parkTransform(mThetaMech, mVa / mBase_v, mVb / mBase_v, mVc / mBase_v)(2);
		mVfd = mExciter.step(mVd, mVq, 1, dt);
	}

Viviane Sapucaia's avatar
Viviane Sapucaia committed
246
	// Calculate rotor flux likanges
247
	if (mNumDampingWindings == 2)
Viviane Sapucaia's avatar
Viviane Sapucaia committed
248
249
250
251
252
253
254
	{
		C_flux <<
			0,
			0,
			mVfd,
			0;

255
		mDqStatorCurrents <<
Viviane Sapucaia's avatar
Viviane Sapucaia committed
256
257
			mIq,
			mId;
258
259

		mDqStatorCurrents_hist <<
Viviane Sapucaia's avatar
Viviane Sapucaia committed
260
261
262
			mIq_hist,
			mId_hist;

263
		mRotorFlux <<
Viviane Sapucaia's avatar
Viviane Sapucaia committed
264
265
266
267
268
			mPsikq1,
			mPsikq2,
			mPsifd,
			mPsikd;

269
270
271
272
		if (numMethod == NumericalMethod::Trapezoidal_flux)
			mRotorFlux = Trapezoidal(mRotorFlux, A_flux, B_flux, C_flux, dt*mBase_OmElec, mDqStatorCurrents, mDqStatorCurrents_hist);
		else if (numMethod == NumericalMethod::Euler)
			mRotorFlux = Euler(mRotorFlux, A_flux, B_flux, C_flux, dt*mBase_OmElec, mDqStatorCurrents);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
273

274
275
276
277
		mPsikq1 = mRotorFlux(0);
		mPsikq2 = mRotorFlux(1);
		mPsifd = mRotorFlux(2);
		mPsikd = mRotorFlux(3);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
278

Viviane Sapucaia's avatar
Viviane Sapucaia committed
279
	}
Viviane Sapucaia's avatar
Viviane Sapucaia committed
280
281
282
283
284
285
286
	else
	{
		C_flux <<
			0,
			mVfd,
			0;

287
		mDqStatorCurrents <<
Viviane Sapucaia's avatar
Viviane Sapucaia committed
288
289
			mIq,
			mId;
290
		mDqStatorCurrents_hist <<
Viviane Sapucaia's avatar
Viviane Sapucaia committed
291
292
293
			mIq_hist,
			mId_hist;

294
		mRotorFlux <<
Viviane Sapucaia's avatar
Viviane Sapucaia committed
295
296
297
298
			mPsikq1,
			mPsifd,
			mPsikd;

299
300
301
302
		if (numMethod == NumericalMethod::Trapezoidal_flux)
			mRotorFlux = Trapezoidal(mRotorFlux, A_flux, B_flux, C_flux, dt*mBase_OmElec, mDqStatorCurrents, mDqStatorCurrents_hist);
		else if (numMethod == NumericalMethod::Euler)
			mRotorFlux = Euler(mRotorFlux, A_flux, B_flux, C_flux, dt*mBase_OmElec, mDqStatorCurrents);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
303

304
305
306
		mPsikq1 = mRotorFlux(0);
		mPsifd = mRotorFlux(1);
		mPsikd = mRotorFlux(2);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
307
	}
308

Steffen Vogel's avatar
Steffen Vogel committed
309

310
	// Calculate dynamic flux likages
311
	if (mNumDampingWindings == 2) {
Viviane Sapucaia's avatar
Viviane Sapucaia committed
312
		mDPsiq = mDLmq*(mPsikq1 / mLlkq1) + mDLmq*(mPsikq2 / mLlkq2);
313
		mPsimq = mDLmq*(mPsikq1 / mLlkq1 + mPsikq2 / mLlkq2 + mIq);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
314
315
316
	}
	else {
		mDPsiq = mDLmq*(mPsikq1 / mLlkq1);
317
		mPsimq = mDLmq*(mPsikq1 / mLlkq1 + mIq);
Viviane Sapucaia's avatar
Viviane Sapucaia committed
318
	}
319
	mDPsid = mDLmd*(mPsifd / mLlfd) + mDLmd*(mPsikd / mLlkd);
320
	mPsimd = mDLmd*(mPsifd / mLlfd + mPsikd / mLlkd + mId);
Steffen Vogel's avatar
Steffen Vogel committed
321

322
323

	// Calculate dynamic voltages
324
	if (mNumDampingWindings == 2) {
Viviane Sapucaia's avatar
Viviane Sapucaia committed
325
326
327
328
329
330
		mDVq = mOmMech*mDPsid + mDLmq*mRkq1*(mDPsiq - mPsikq1) / (mLlkq1*mLlkq1) +
			mDLmq*mRkq2*(mDPsiq - mPsikq2) / (mLlkq2*mLlkq2) + (mRkq1 / (mLlkq1*mLlkq1) + mRkq2 / (mLlkq2*mLlkq2))*mDLmq*mDLmq*mIq;
	}
	else {
		mDVq = mOmMech*mDPsid + mDLmq*mRkq1*(mDPsiq - mPsikq1) / (mLlkq1*mLlkq1) + (mRkq1 / (mLlkq1*mLlkq1))*mDLmq*mDLmq*mIq;
	}
331
332
	mDVd = -mOmMech*mDPsiq + mDLmd*mRkd*(mDPsid - mPsikd) / (mLlkd*mLlkd) + (mDLmd / mLlfd)*mVfd +
		mDLmd*mRfd*(mDPsid - mPsifd) / (mLlfd*mLlfd) + (mRfd / (mLlfd*mLlfd) + mRkd / (mLlkd*mLlkd))*mDLmd*mDLmd*mId;
333
334
335
336

	mDVa = inverseParkTransform(mThetaMech, mDVq, mDVd, 0)(0);
	mDVb = inverseParkTransform(mThetaMech, mDVq, mDVd, 0)(1);
	mDVc = inverseParkTransform(mThetaMech, mDVq, mDVd, 0)(2);
337
338
339
340
341
342
	mDVabc_hist = mDVabc;
	mDVabc <<
		mDVa,
		mDVb,
		mDVc;

343
344
345



346

Viviane Sapucaia's avatar
Viviane Sapucaia committed
347
348
349
}


350
void VoltageBehindReactanceEMT::postStep(SystemModel& system) {
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
	if (mNode1 >= 0) {
		mVa = system.getRealFromLeftSideVector(mNode1);
	}
	else {
		mVa = 0;
	}
	if (mNode2 >= 0) {
		mVb = system.getRealFromLeftSideVector(mNode2);
	}
	else {
		mVb = 0;
	}
	if (mNode3 >= 0) {
		mVc = system.getRealFromLeftSideVector(mNode3);
	}
	else {
		mVc = 0;
	}
369
}
370

371
372
373
374
375
376
377
378
379
380
void VoltageBehindReactanceEMT::CalculateLandpL() {
	mDInductanceMat <<
		mLl + mLa - mLb*cos(2 * mThetaMech), -mLa / 2 - mLb*cos(2 * mThetaMech - 2 * PI / 3), -mLa / 2 - mLb*cos(2 * mThetaMech + 2 * PI / 3),
		-mLa / 2 - mLb*cos(2 * mThetaMech - 2 * PI / 3), mLl + mLa - mLb*cos(2 * mThetaMech - 4 * PI / 3), -mLa / 2 - mLb*cos(2 * mThetaMech),
		-mLa / 2 - mLb*cos(2 * mThetaMech + 2 * PI / 3), -mLa / 2 - mLb*cos(2 * mThetaMech), mLl + mLa - mLb*cos(2 * mThetaMech + 4 * PI / 3);
	pmDInductanceMat <<
		mLb*sin(2 * mThetaMech), mLb*sin(2 * mThetaMech - 2 * PI / 3), mLb*sin(2 * mThetaMech + 2 * PI / 3),
		mLb*sin(2 * mThetaMech - 2 * PI / 3), mLb*sin(2 * mThetaMech - 4 * PI / 3), mLb*sin(2 * mThetaMech),
		mLb*sin(2 * mThetaMech + 2 * PI / 3), mLb*sin(2 * mThetaMech), mLb*sin(2 * mThetaMech + 4 * PI / 3);
	pmDInductanceMat = pmDInductanceMat * 2 * mOmMech;
381
382
383
}


Markus Mirz's avatar
Markus Mirz committed
384
Matrix VoltageBehindReactanceEMT::parkTransform(Real theta, Real a, Real b, Real c) {
385

Markus Mirz's avatar
Markus Mirz committed
386
	Matrix dq0vector(3, 1);
387

388
	Real q, d, zero;
389

Steffen Vogel's avatar
Steffen Vogel committed
390
	q = 2. / 3. * cos(theta)*a + 2. / 3. * cos(theta - 2. * M_PI / 3.)*b + 2. / 3. * cos(theta + 2. * M_PI / 3.)*c;
391
	d = 2. / 3. * sin(theta)*a + 2. / 3. * sin(theta - 2. * M_PI / 3.)*b + 2. / 3. * sin(theta + 2. * M_PI / 3.)*c;
392
	zero = 1. / 3.*a + 1. / 3.*b + 1. / 3.*c;
393

394
395
	dq0vector << q,
		d,
396
		zero;
397
398
399
400
401
402

	return dq0vector;
}



Markus Mirz's avatar
Markus Mirz committed
403
Matrix VoltageBehindReactanceEMT::inverseParkTransform(Real theta, Real q, Real d, Real zero) {
404

Markus Mirz's avatar
Markus Mirz committed
405
	Matrix abcVector(3, 1);
406

Markus Mirz's avatar
Markus Mirz committed
407
	Real a, b, c;
408

Viviane Sapucaia's avatar
Viviane Sapucaia committed
409
410
411
	a = cos(theta)*q + sin(theta)*d + 1.*zero;
	b = cos(theta - 2. * M_PI / 3.)*q + sin(theta - 2. * M_PI / 3.)*d + 1.*zero;
	c = cos(theta + 2. * M_PI / 3.)*q + sin(theta + 2. * M_PI / 3.)*d + 1.*zero;
412
413
414
415
416
417

	abcVector << a,
		b,
		c;

	return abcVector;
Viviane Sapucaia's avatar
Viviane Sapucaia committed
418
419
}