Browse Source
add enable/disable to mavlinkcheckbox modify my button to curved add delay to progress reporter dialog. to ensure correct parent Fix Mount screen for AP Fix Hardware screen Text display roi difrently modify HIL/Quad Hil update dataflashlog format (thanks randy) update mavcmd format for roimaster
Michael Oborne
13 years ago
31 changed files with 2248 additions and 1590 deletions
@ -0,0 +1,364 @@
@@ -0,0 +1,364 @@
|
||||
using System; |
||||
using System.Collections.Generic; |
||||
using System.Linq; |
||||
using System.Text; |
||||
using Sharp3D.Math.Core; |
||||
|
||||
namespace ArdupilotMega.HIL |
||||
{ |
||||
class FlashQuad |
||||
{ |
||||
public ahrsc ahrs = new ahrsc() ;// :AHRS; |
||||
public Vector3D loc = new Vector3D() ;// :Location; |
||||
public param g = new param() ;// :Parameters; |
||||
//public var apm_rc :APM_RC; |
||||
//public var motor_filter_0 :AverageFilter; |
||||
//public var motor_filter_1 :AverageFilter; |
||||
//public var motor_filter_2 :AverageFilter; |
||||
//public var motor_filter_3 :AverageFilter; |
||||
public Vector3D drag = new Vector3D();// :Vector3D; // |
||||
public Vector3D airspeed = new Vector3D();// :Vector3D; // |
||||
//public var thrust :Vector3D; // |
||||
public Vector3D position = new Vector3D();// :Vector3D; // |
||||
public Vector3D velocity = new Vector3D();// :Vector3D; |
||||
//public var velocity_old :Vector3D; |
||||
//public var wind :Point; // |
||||
public Vector3D rot_accel = new Vector3D();// :Vector3D; // |
||||
public Vector3D angle3D = new Vector3D();// :Vector3D; // |
||||
//public var windGenerator :Wind; // |
||||
|
||||
public double gravity = 980.5; |
||||
public double thrust_scale = 0.4; |
||||
public double throttle = 500; |
||||
public double rotation_bias = 1; |
||||
|
||||
private double _jump_z = 0; |
||||
private Vector3D v3 = new Vector3D(); |
||||
|
||||
public class ahrsc |
||||
{ |
||||
public Matrix3D dcm = new Matrix3D(Matrix3D.Identity); |
||||
public Vector3D gyro = new Vector3D(); |
||||
public Vector3D omega = new Vector3D(); |
||||
public Vector3D accel = new Vector3D(); |
||||
public Vector3D rotation_speed = new Vector3D(); |
||||
|
||||
public double roll_sensor; |
||||
public double pitch_sensor; |
||||
public double yaw_sensor; |
||||
} |
||||
|
||||
public class param |
||||
{ |
||||
// --------------------------------------------- |
||||
// Sim Details controls |
||||
// --------------------------------------------- |
||||
public int sim_iterations = 99999; |
||||
public int sim_speed = 1; |
||||
|
||||
public double windSpeedMin = 150; |
||||
public double windSpeedMax = 200; |
||||
public double windPeriod = 30000; |
||||
public double windDir = 45; |
||||
|
||||
public double airDensity = 1.184; |
||||
//public var crossSection :Number = 0.015; |
||||
public double crossSection = 0.012; |
||||
public double dragCE = 0.20; |
||||
public double speed_filter_size = 2; |
||||
public double motor_kv = 1000; |
||||
public double moment = 3; |
||||
public double mass = 500; |
||||
public int esc_delay = 12; |
||||
|
||||
// ----------------------------------------- |
||||
// Inertial control |
||||
// ----------------------------------------- |
||||
public double speed_correction_z = 0.0350; |
||||
public double xy_speed_correction = 0.030; |
||||
public double xy_offset_correction = 0.00001; |
||||
public double xy_pos_correction = 0.08; |
||||
|
||||
public double z_offset_correction = 0.00004; |
||||
public double z_pos_correction = 0.2; |
||||
|
||||
public double accel_bias_x = .9; |
||||
public double accel_bias_z = .9; |
||||
public double accel_bias_y = .9; |
||||
} |
||||
|
||||
double scale_rc(int sn, float servo, float min, float max) |
||||
{ |
||||
float min_pwm = 1000; |
||||
float max_pwm = 2000; |
||||
//'''scale a PWM value''' # default to servo range of 1000 to 2000 |
||||
if (MainV2.comPort.param.Count > 0) |
||||
{ |
||||
min_pwm = int.Parse(MainV2.comPort.param["RC3_MIN"].ToString()); |
||||
max_pwm = int.Parse(MainV2.comPort.param["RC3_MAX"].ToString()); |
||||
} |
||||
|
||||
float p = (servo - min_pwm) / (max_pwm - min_pwm); |
||||
|
||||
float v = min + p * (max - min); |
||||
|
||||
if (v < min) |
||||
v = min; |
||||
if (v > max) |
||||
v = max; |
||||
return v * 1000; |
||||
} |
||||
|
||||
public void update(ref double[] motor_output, double dt) |
||||
{ |
||||
var _thrust = 0.0; |
||||
rot_accel = new Vector3D(0,0,0); |
||||
angle3D.X = angle3D.Y = 0; |
||||
angle3D.Z = 1; |
||||
|
||||
// wind = windGenerator.read(); |
||||
|
||||
// ESC's moving average filter |
||||
// var motor_output:Array = new Array(4); |
||||
// motor_output[0] = motor_filter_0.apply(apm_rc.get_motor_output(0)); |
||||
// motor_output[1] = motor_filter_1.apply(apm_rc.get_motor_output(1)); |
||||
// motor_output[2] = motor_filter_2.apply(apm_rc.get_motor_output(2)); |
||||
// motor_output[3] = motor_filter_3.apply(apm_rc.get_motor_output(3)); |
||||
|
||||
|
||||
for (int i = 0; i < motor_output.Length; i++) |
||||
{ |
||||
if (motor_output[i] <= 0.0) |
||||
{ |
||||
motor_output[i] = 0; |
||||
} |
||||
else |
||||
{ |
||||
motor_output[i] = scale_rc(i, (float)motor_output[i], 0.0f, 1.0f); |
||||
//servos[i] = motor_speed[i]; |
||||
} |
||||
} |
||||
|
||||
/* |
||||
2 |
||||
|
||||
1 0 |
||||
|
||||
3 |
||||
|
||||
*/ |
||||
|
||||
// setup motor rotations |
||||
rot_accel.X -= g.motor_kv * motor_output[0]; // roll |
||||
rot_accel.X += g.motor_kv * motor_output[1]; |
||||
rot_accel.Y -= g.motor_kv * motor_output[3]; |
||||
rot_accel.Y += g.motor_kv * motor_output[2]; |
||||
|
||||
rot_accel.Z += g.motor_kv * motor_output[0] * .08; // YAW |
||||
rot_accel.Z += g.motor_kv * motor_output[1] * .08; |
||||
rot_accel.Z -= g.motor_kv * motor_output[2] * .08; |
||||
rot_accel.Z -= g.motor_kv * motor_output[3] * .08; |
||||
|
||||
rot_accel.X /= g.moment; |
||||
rot_accel.Y /= g.moment; |
||||
rot_accel.Z /= g.moment; |
||||
|
||||
//# rotational air resistance |
||||
|
||||
// Gyro is the rotation speed in deg/s |
||||
// update rotational rates in body frame |
||||
ahrs.gyro.X += rot_accel.X * dt; |
||||
ahrs.gyro.Y += rot_accel.Y * dt; |
||||
ahrs.gyro.Z += rot_accel.Z * dt; |
||||
|
||||
//ahrs.gyro.z += 200; |
||||
ahrs.gyro.Z *= .995;// some drag |
||||
|
||||
// ahrs.dcm = Matrix3D.Identity; |
||||
|
||||
// move earth frame to body frame |
||||
Vector3D tmp = Matrix3D.Transform(ahrs.dcm,ahrs.gyro) ;//ahrs.dcm.transformVector(ahrs.gyro); |
||||
|
||||
// update attitude: |
||||
ahrs.dcm += new Matrix3D(new Vector3D((tmp.X/100) * dt,0,0),new Vector3D(0,(tmp.Y/100) * dt,0),new Vector3D(0,0,(tmp.Z/100) * dt)); |
||||
|
||||
//ahrs.dcm.appendRotation((tmp.X/100) * dt, Vector3D.X_AXIS); // ROLL |
||||
//ahrs.dcm.appendRotation((tmp.Y/100) * dt, Vector3D.Y_AXIS); // PITCH |
||||
//ahrs.dcm.appendRotation((tmp.Z/100) * dt, Vector3D.Z_AXIS); // YAW |
||||
|
||||
// ------------------------------------ |
||||
// calc thrust |
||||
// ------------------------------------ |
||||
|
||||
//get_motor_output returns 0 : 1000 |
||||
_thrust += motor_output[0] * thrust_scale; |
||||
_thrust += motor_output[1] * thrust_scale; |
||||
_thrust += motor_output[2] * thrust_scale; |
||||
_thrust += motor_output[3] * thrust_scale; |
||||
|
||||
Vector3D accel_body = new Vector3D(0, 0, (_thrust * -.9) / g.mass); |
||||
//var accel_body:Vector3D = new Vector3D(0, 0, 0); |
||||
Vector3D accel_earth = Matrix3D.Transform(ahrs.dcm,(accel_body)); |
||||
angle3D = Matrix3D.Transform(ahrs.dcm,(angle3D)); |
||||
|
||||
//trace(ahrs.gyro.y, accel_earth.x); |
||||
|
||||
//trace(ahrs.gyro.x, ahrs.gyro.y, ahrs.gyro.z); |
||||
|
||||
// ------------------------------------ |
||||
// calc copter velocity |
||||
// ------------------------------------ |
||||
// calc Drag |
||||
drag.X = .5 * g.airDensity * airspeed.X * airspeed.X * g.dragCE * g.crossSection; |
||||
drag.Y = .5 * g.airDensity * airspeed.Y * airspeed.Y * g.dragCE * g.crossSection; |
||||
drag.Z = .5 * g.airDensity * airspeed.Z * airspeed.Z * g.dragCE * g.crossSection; |
||||
|
||||
///* |
||||
// this calulation includes wind |
||||
if(airspeed.X >= 0) |
||||
accel_earth.X -= drag.X; |
||||
else |
||||
accel_earth.X += drag.X; |
||||
|
||||
// Add in Drag |
||||
if(airspeed.Y >= 0) |
||||
accel_earth.Y -= drag.Y; |
||||
else |
||||
accel_earth.Y += drag.Y; |
||||
|
||||
if(airspeed.Z <= 0) |
||||
accel_earth.Z -= drag.Z; |
||||
else |
||||
accel_earth.Z += drag.Z; |
||||
//*/ |
||||
|
||||
// hacked vert disturbance |
||||
accel_earth.Z += _jump_z * dt; |
||||
_jump_z *= .999; |
||||
|
||||
|
||||
// Add in Gravity |
||||
accel_earth.Z += gravity; |
||||
|
||||
if(accel_earth.Z < 0) |
||||
accel_earth.Z *=.9; |
||||
|
||||
|
||||
if(position.Z <=.11 && accel_earth.Z > 0){ |
||||
accel_earth.Z = 0; |
||||
} |
||||
|
||||
velocity.X += (accel_earth.X * dt); // + : Forward (North) |
||||
velocity.Y += (accel_earth.Y * dt); // + : Right (East) |
||||
velocity.Z -= (accel_earth.Z * dt); // + : Up |
||||
|
||||
|
||||
//trace(Math.floor(velocity.x),Math.floor(velocity.y),Math.floor(velocity.z)); |
||||
|
||||
// ------------------------------------ |
||||
// calc inertia |
||||
// ------------------------------------ |
||||
|
||||
// work out acceleration as seen by the accelerometers. It sees the kinematic |
||||
// acceleration (ie. real movement), plus gravity |
||||
Matrix3D dcm_t = ahrs.dcm.Clone(); |
||||
dcm_t.Transpose(); |
||||
double t = accel_earth.Z; |
||||
accel_earth.Z -= gravity; |
||||
ahrs.accel = Matrix3D.Transform(dcm_t,(accel_earth)); |
||||
|
||||
ahrs.accel *= 0.01; |
||||
|
||||
//ahrs.accel = accel_earth.clone(); |
||||
ahrs.accel.X *= g.accel_bias_x; |
||||
ahrs.accel.Y *= g.accel_bias_y; |
||||
ahrs.accel.Z *= g.accel_bias_z; |
||||
|
||||
|
||||
|
||||
// ------------------------------------ |
||||
// calc Position |
||||
// ------------------------------------ |
||||
position.Y += velocity.X * dt; |
||||
position.X += velocity.Y * dt; |
||||
position.Z += velocity.Z * dt; |
||||
position.Z = Math.Min(position.Z, 4000); |
||||
|
||||
// XXX Force us to 3m above ground |
||||
//position.z = 300; |
||||
|
||||
airspeed.X = (velocity.X);// - wind.x); |
||||
airspeed.Y = (velocity.Y);// - wind.y); |
||||
airspeed.Z = velocity.Z; |
||||
|
||||
// Altitude |
||||
// -------- |
||||
if(position.Z <=.1){ |
||||
position.Z = .1; |
||||
velocity.X = 0; |
||||
velocity.Y = 0; |
||||
velocity.Z = 0; |
||||
airspeed.X = 0; |
||||
airspeed.Y = 0; |
||||
airspeed.Z = 0; |
||||
//ahrs.init(); |
||||
} |
||||
|
||||
|
||||
// get omega - the simulated Gyro output |
||||
ahrs.omega.X = radiansx100(ahrs.gyro.X); |
||||
ahrs.omega.Y = radiansx100(ahrs.gyro.Y); |
||||
ahrs.omega.Z = radiansx100(ahrs.gyro.Z); |
||||
|
||||
// get the Eulers output |
||||
v3 = new Vector3D(Math.Atan2(-ahrs.dcm.M23, ahrs.dcm.M22), Math.Asin(ahrs.dcm.M21), Math.Atan2(-ahrs.dcm.M31, ahrs.dcm.M11));// ahrs.dcm.decompose(); |
||||
ahrs.roll_sensor = Math.Floor(degrees(v3.X) * 100); |
||||
ahrs.pitch_sensor = Math.Floor(degrees(v3.Y) * 100); |
||||
ahrs.yaw_sensor = Math.Floor(degrees(v3.Z) * 100); |
||||
|
||||
// store the position for the GPS object |
||||
loc.X = position.X; |
||||
loc.Y = position.Y; |
||||
loc.Z = position.Z; |
||||
} |
||||
|
||||
public double constrain(double val, double min, double max) |
||||
{ |
||||
val = Math.Max(val, min); |
||||
val = Math.Min(val, max); |
||||
return val; |
||||
} |
||||
|
||||
public double wrap_180(int error) |
||||
{ |
||||
if (error > 18000) error -= 36000; |
||||
if (error < -18000) error += 36000; |
||||
return error; |
||||
} |
||||
|
||||
public int wrap_360(int error) |
||||
{ |
||||
if (error > 36000) error -= 36000; |
||||
if (error < 0) error += 36000; |
||||
return error; |
||||
} |
||||
|
||||
public double radiansx100(double n) |
||||
{ |
||||
return 0.000174532925 * n; |
||||
} |
||||
|
||||
public double degreesx100(double r) |
||||
{ |
||||
return r * 5729.57795; |
||||
} |
||||
public double degrees(double r) |
||||
{ |
||||
return r * 57.2957795; |
||||
} |
||||
public double radians(double n) |
||||
{ |
||||
return 0.0174532925 * n; |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,179 @@
@@ -0,0 +1,179 @@
|
||||
using System; |
||||
using System.Collections.Generic; |
||||
using System.Linq; |
||||
using System.Text; |
||||
using YLScsDrawing.Drawing3d; |
||||
|
||||
namespace ArdupilotMega.HIL |
||||
{ |
||||
public class Matrix3 |
||||
{ |
||||
// '''a 3x3 matrix, intended as a rotation matrix''' |
||||
|
||||
Matrix3 self; |
||||
Vector3 a; |
||||
Vector3 b; |
||||
Vector3 c; |
||||
|
||||
public Matrix3() |
||||
{ |
||||
self = this; |
||||
self.identity(); |
||||
} |
||||
|
||||
public Matrix3(Vector3 a, Vector3 b, Vector3 c) |
||||
{ |
||||
self = this; |
||||
|
||||
this.a = a; |
||||
this.b = b; |
||||
this.c = c; |
||||
} |
||||
|
||||
public new string ToString() |
||||
{ |
||||
return String.Format("Matrix3(({0}, {1}, {2}), ({3}, {4}, {5}), ({6}, {7}, {8}))", |
||||
self.a.x, self.a.y, self.a.z, |
||||
self.b.x, self.b.y, self.b.z, |
||||
self.c.x, self.c.y, self.c.z); |
||||
} |
||||
|
||||
public void identity() |
||||
{ |
||||
self.a = new Vector3(1, 0, 0); |
||||
self.b = new Vector3(0, 1, 0); |
||||
self.c = new Vector3(0, 0, 1); |
||||
} |
||||
|
||||
public Matrix3 transposed() |
||||
{ |
||||
return new Matrix3(new Vector3(self.a.x, self.b.x, self.c.x), |
||||
new Vector3(self.a.y, self.b.y, self.c.y), |
||||
new Vector3(self.a.z, self.b.z, self.c.z)); |
||||
} |
||||
|
||||
|
||||
public void from_euler(double roll, double pitch, double yaw) |
||||
{ |
||||
// '''fill the matrix from Euler angles in radians''' |
||||
double cp = Utils.cos(pitch); |
||||
double sp = Utils.sin(pitch); |
||||
double sr = Utils.sin(roll); |
||||
double cr = Utils.cos(roll); |
||||
double sy = Utils.sin(yaw); |
||||
double cy = Utils.cos(yaw); |
||||
|
||||
self.a.x = cp * cy; |
||||
self.a.y = (sr * sp * cy) - (cr * sy); |
||||
self.a.z = (cr * sp * cy) + (sr * sy); |
||||
self.b.x = cp * sy; |
||||
self.b.y = (sr * sp * sy) + (cr * cy); |
||||
self.b.z = (cr * sp * sy) - (sr * cy); |
||||
self.c.x = -sp; |
||||
self.c.y = sr * cp; |
||||
self.c.z = cr * cp; |
||||
} |
||||
|
||||
public void to_euler(ref double roll, ref double pitch, ref double yaw) |
||||
{ |
||||
// '''find Euler angles for the matrix''' |
||||
if (self.c.x >= 1.0) |
||||
pitch = Math.PI; |
||||
else if (self.c.x <= -1.0) |
||||
pitch = -Math.PI; |
||||
else |
||||
pitch = -Utils.asin(self.c.x); |
||||
roll = Utils.atan2(self.c.y, self.c.z); |
||||
yaw = Utils.atan2(self.b.x, self.a.x); |
||||
//return (roll, pitch, yaw) |
||||
} |
||||
|
||||
public static Matrix3 operator +(Matrix3 self, Matrix3 m) |
||||
{ |
||||
return new Matrix3(self.a + m.a, self.b + m.b, self.c + m.c); |
||||
} |
||||
|
||||
public static Matrix3 operator -(Matrix3 self, Matrix3 m) |
||||
{ |
||||
return new Matrix3(self.a - m.a, self.b - m.b, self.c - m.c); |
||||
} |
||||
|
||||
public static Vector3 operator *(Matrix3 self, Vector3 v) |
||||
{ |
||||
return new Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z, |
||||
self.b.x * v.x + self.b.y * v.y + self.b.z * v.z, |
||||
self.c.x * v.x + self.c.y * v.y + self.c.z * v.z); |
||||
} |
||||
|
||||
public static Matrix3 operator *(Matrix3 self, Matrix3 m) |
||||
{ |
||||
return new Matrix3(new Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x, |
||||
self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y, |
||||
self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z), |
||||
new Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x, |
||||
self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y, |
||||
self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z), |
||||
new Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x, |
||||
self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y, |
||||
self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z)); |
||||
} |
||||
|
||||
public static Matrix3 operator *(Matrix3 self, double v) |
||||
{ |
||||
|
||||
return new Matrix3(self.a * v, self.b * v, self.c * v); |
||||
} |
||||
|
||||
public static Matrix3 operator /(Matrix3 self, double v) |
||||
{ |
||||
return new Matrix3(self.a / v, self.b / v, self.c / v); |
||||
} |
||||
|
||||
public static Matrix3 operator -(Matrix3 self) |
||||
{ |
||||
return new Matrix3(-self.a, -self.b, -self.c); |
||||
} |
||||
|
||||
public Matrix3 copy() |
||||
{ |
||||
return new Matrix3(self.a.copy(), self.b.copy(), self.c.copy()); |
||||
} |
||||
|
||||
public void rotate(Vector3 g) |
||||
{ |
||||
// '''rotate the matrix by a given amount on 3 axes''' |
||||
Matrix3 temp_matrix = new Matrix3(self.a.copy(), self.b.copy(), self.c.copy()); |
||||
|
||||
temp_matrix.a.x = a.y * g.z - a.z * g.y; |
||||
temp_matrix.a.y = a.z * g.x - a.x * g.z; |
||||
temp_matrix.a.z = a.x * g.y - a.y * g.x; |
||||
temp_matrix.b.x = b.y * g.z - b.z * g.y; |
||||
temp_matrix.b.y = b.z * g.x - b.x * g.z; |
||||
temp_matrix.b.z = b.x * g.y - b.y * g.x; |
||||
temp_matrix.c.x = c.y * g.z - c.z * g.y; |
||||
temp_matrix.c.y = c.z * g.x - c.x * g.z; |
||||
temp_matrix.c.z = c.x * g.y - c.y * g.x; |
||||
self.a += temp_matrix.a; |
||||
self.b += temp_matrix.b; |
||||
self.c += temp_matrix.c; |
||||
} |
||||
|
||||
public void normalize() |
||||
{ |
||||
// '''re-normalise a rotation matrix''' |
||||
Vector3 error = self.a * self.b; |
||||
Vector3 t0 = self.a - (self.b * (0.5 * error)); |
||||
Vector3 t1 = self.b - (self.a * (0.5 * error)); |
||||
Vector3 t2 = t0 % t1; |
||||
self.a = t0 * (1.0 / t0.length()); |
||||
self.b = t1 * (1.0 / t1.length()); |
||||
self.c = t2 * (1.0 / t2.length()); |
||||
} |
||||
|
||||
public double trace() |
||||
{ |
||||
// '''the trace of the matrix''' |
||||
return self.a.x + self.b.y + self.c.z; |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,107 @@
@@ -0,0 +1,107 @@
|
||||
using System; |
||||
using System.Collections.Generic; |
||||
using System.Linq; |
||||
using System.Text; |
||||
|
||||
namespace ArdupilotMega.HIL |
||||
{ |
||||
public class Vector3 |
||||
{ |
||||
Vector3 self; |
||||
public double x; |
||||
public double y; |
||||
public double z; |
||||
|
||||
public Vector3(double x = 0, double y = 0, double z = 0) |
||||
{ |
||||
self = this; |
||||
this.x = x; |
||||
this.y = y; |
||||
this.z = z; |
||||
} |
||||
|
||||
public new string ToString() { |
||||
return String.Format("Vector3({0}, {1}, {2})",self.x, |
||||
self.y, |
||||
self.z); |
||||
} |
||||
|
||||
public static Vector3 operator +(Vector3 self, Vector3 v) { |
||||
|
||||
return new Vector3(self.x + v.x, |
||||
self.y + v.y, |
||||
self.z + v.z); |
||||
} |
||||
|
||||
|
||||
public static Vector3 operator -(Vector3 self, Vector3 v) { |
||||
return new Vector3(self.x - v.x, |
||||
self.y - v.y, |
||||
self.z - v.z); |
||||
} |
||||
|
||||
public static Vector3 operator -(Vector3 self) { |
||||
return new Vector3(-self.x, -self.y, -self.z); |
||||
} |
||||
|
||||
|
||||
public static Vector3 operator *(Vector3 self, Vector3 v) { |
||||
// '''dot product''' |
||||
return new Vector3(self.x*v.x + self.y*v.y + self.z*v.z); |
||||
|
||||
} |
||||
|
||||
public static Vector3 operator *(Vector3 self, double v) { |
||||
return new Vector3(self.x * v, |
||||
self.y * v, |
||||
self.z * v); |
||||
} |
||||
|
||||
public static Vector3 operator *(double v, Vector3 self) |
||||
{ |
||||
return (self * v); |
||||
} |
||||
|
||||
public static Vector3 operator /(Vector3 self, double v) { |
||||
return new Vector3(self.x / v, |
||||
self.y / v, |
||||
self.z / v); |
||||
} |
||||
|
||||
public static Vector3 operator %(Vector3 self, Vector3 v) { |
||||
// '''cross product''' |
||||
return new Vector3(self.y*v.z - self.z*v.y, |
||||
self.z*v.x - self.x*v.z, |
||||
self.x*v.y - self.y*v.x); |
||||
} |
||||
|
||||
public Vector3 copy() { |
||||
return new Vector3(self.x, self.y, self.z); |
||||
} |
||||
|
||||
|
||||
public double length() { |
||||
return Math.Sqrt(self.x*self.x + self.y*self.y + self.z*self.z); |
||||
} |
||||
|
||||
public void zero() { |
||||
self.x = self.y = self.z = 0; |
||||
} |
||||
|
||||
// public double angle (Vector3 self, Vector3 v) { |
||||
// '''return the angle between this vector and another vector''' |
||||
// return Math.Acos(self * v) / (self.length() * v.length()); |
||||
// } |
||||
|
||||
public Vector3 normalized(){ |
||||
return self / self.length(); |
||||
} |
||||
|
||||
public void normalize() { |
||||
Vector3 v = self.normalized(); |
||||
self.x = v.x; |
||||
self.y = v.y; |
||||
self.z = v.z; |
||||
} |
||||
} |
||||
} |
||||
Loading…
Reference in new issue