Distance between two line segments - objective-c

Question

EDIT: PROBLEM SOLVED. This code works properly !
I translated this python code Shortest distance between two line segments (answered by Fnord) to Objective-C in order to find the shortest distance between two line segments. However the distances are incorrect. Could you please help me find what is wrong?
Matrix.h:
#import <Cocoa/Cocoa.h>
#interface Matrix : NSObject
#property NSUInteger m;
#property NSUInteger n;
#property NSMutableArray *rows;
- (id)initWithSizeM:(NSUInteger)m N:(NSUInteger)n;
- (CGFloat)determinant3x3;
#end
Matrix.m:
#import <Cocoa/Cocoa.h>
#import "Matrix.h"
#implementation Matrix;
- (id)initWithSizeM:(NSUInteger)m N:(NSUInteger)n {
self = [super init];
_m = m;
_n = n;
_rows = [NSMutableArray new];
for (int i = 0; i < m; i ++) {
[_rows addObject:[NSMutableArray new]];
for (int j = 0; j < n; j ++) {
CGFloat value = 0.0;
[_rows[i] addObject:[NSNumber numberWithFloat:value]];
}
}
return self;
}
- (CGFloat)determinant3x3 {
CGFloat a = [_rows[0][0] floatValue];
CGFloat b = [_rows[0][1] floatValue];
CGFloat c = [_rows[0][2] floatValue];
CGFloat d = [_rows[1][0] floatValue];
CGFloat e = [_rows[1][1] floatValue];
CGFloat f = [_rows[1][2] floatValue];
CGFloat g = [_rows[2][0] floatValue];
CGFloat h = [_rows[2][1] floatValue];
CGFloat i = [_rows[2][2] floatValue];
return a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h;
}
#end
PointCartesian.h:
#import <Cocoa/Cocoa.h>
#import "Vector.h"
#class Vertex;
#interface PointCartesian: NSObject
#property CGFloat x;
#property CGFloat y;
#property CGFloat z;
#property NSString *name;
- (id)initWithCoordinatesX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z;
- (Vector*)toVector;
+ (CGFloat)shortestDistanceBetweenTwoSegmentsA0:(PointCartesian*)a0 A1:(PointCartesian*)a1 B0:(PointCartesian*)b0 B1:(PointCartesian*)b1;
#end
PointCartesian.m:
#import <Cocoa/Cocoa.h>
#import "PointCartesian.h"
#import "Vector.h"
#import "Matrix.h"
#implementation PointCartesian;
- (id)initWithCoordinatesX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z {
_x = x;
_y = y;
_z = z;
return self;
}
- (Vector*)toVector {
return [[Vector alloc] initWithX:_x Y:_y Z:_z];
}
+ (CGFloat)shortestDistanceBetweenTwoSegmentsA0:(PointCartesian*)a0 A1:(PointCartesian*)a1 B0:(PointCartesian*)b0 B1:(PointCartesian*)b1 {
BOOL clampA0 = YES;
BOOL clampA1 = YES;
BOOL clampB0 = YES;
BOOL clampB1 = YES;
Vector *A = [Vector vectorFromA:a0 B:a1];
Vector *B = [Vector vectorFromA:b0 B:b1];
CGFloat magA = [A length];
CGFloat magB = [B length];
Vector *_A = [[Vector alloc] initWithX:A.x/magA Y:A.y/magA Z:A.z/magA];
Vector *_B = [[Vector alloc] initWithX:B.x/magB Y:B.y/magB Z:B.z/magB];
Vector *cross = [Vector crossProductA:_A B:_B];
CGFloat denom = pow([cross length], 2);
if (denom == 0) {
CGFloat d0 = [Vector dotProductA:_A B:[Vector vectorFromA:a0 B:b0]];
if (clampA0 || clampA1 || clampB0 || clampB1) {
CGFloat d1 = [Vector dotProductA:_A B:[Vector vectorFromA:a0 B:b1]];
if (d0 <= 0 && 0 >= d1) {
if (clampA0 && clampB1) {
if (floor(d0) < floor(d1)) {
return [[Vector vectorFromA:b0 B:a0] length];
}
return [[Vector vectorFromA:b1 B:a0] length];
}
}
if (d0 >= magA && magA <= d1) {
if (clampA1 && clampB0) {
if (floor(d0) < floor(d1)) {
return [[Vector vectorFromA:b0 B:a1] length];
}
return [[Vector vectorFromA:b1 B:a1] length];
}
}
}
return [[[Vector alloc] initWithX:d0*_A.x + a0.x - b0.x Y:d0*_A.y + a0.y - b0.y Z:d0*_A.z + a0.z - b0.z] length];
}
Vector *t = [Vector vectorFromA:a0 B:b0];
Matrix *MA = [[Matrix alloc] initWithSizeM:3 N:3];
MA.rows[0][0] = [NSNumber numberWithFloat:t.x];
MA.rows[0][1] = [NSNumber numberWithFloat:t.y];
MA.rows[0][2] = [NSNumber numberWithFloat:t.z];
MA.rows[1][0] = [NSNumber numberWithFloat:_B.x];
MA.rows[1][1] = [NSNumber numberWithFloat:_B.y];
MA.rows[1][2] = [NSNumber numberWithFloat:_B.z];
MA.rows[2][0] = [NSNumber numberWithFloat:cross.x];
MA.rows[2][1] = [NSNumber numberWithFloat:cross.y];
MA.rows[2][2] = [NSNumber numberWithFloat:cross.z];
Matrix *MB = [[Matrix alloc] initWithSizeM:3 N:3];
MB.rows[0][0] = [NSNumber numberWithFloat:t.x];
MB.rows[0][1] = [NSNumber numberWithFloat:t.y];
MB.rows[0][2] = [NSNumber numberWithFloat:t.z];
MB.rows[1][0] = [NSNumber numberWithFloat:_A.x];
MB.rows[1][1] = [NSNumber numberWithFloat:_A.y];
MB.rows[1][2] = [NSNumber numberWithFloat:_A.z];
MB.rows[2][0] = [NSNumber numberWithFloat:cross.x];
MB.rows[2][1] = [NSNumber numberWithFloat:cross.y];
MB.rows[2][2] = [NSNumber numberWithFloat:cross.z];
CGFloat detA = [MA determinant3x3];
CGFloat detB = [MB determinant3x3];
CGFloat t0 = detA / denom;
CGFloat t1 = detB / denom;
PointCartesian *pA = [[PointCartesian alloc] initWithCoordinatesX:_A.x*t0 + a0.x Y:_A.y*t0 + a0.y Z:_A.z*t0 + a0.z];
PointCartesian *pB = [[PointCartesian alloc] initWithCoordinatesX:_B.x*t1 + b0.x Y:_B.y*t1 + b0.y Z:_B.z*t1 + b0.z];
if (clampA0 || clampA1 || clampB0 || clampB1) {
if (clampA0 && t0 < 0) {
pA = a0;
} else if (clampA1 && t0 > magA) {
pA = a1;
}
if (clampB0 && t1 < 0) {
pB = b0;
} else if (clampB1 && t1 > magB) {
pB = b1;
}
if ((clampA0 && t0 < 0) || (clampA1 && t0 > magA)) {
CGFloat dot = [Vector dotProductA:_B B:[Vector vectorFromA:b0 B:pA]];
if (clampB0 && dot < 0) {
dot = 0;
} else if (clampB1 && dot > magB) {
dot = magB;
}
pB = [[PointCartesian alloc] initWithCoordinatesX:b0.x + _B.x*dot Y:b0.y + _B.y*dot Z:b0.z + _B.z*dot];
}
if ((clampB0 && t1 < 0) || (clampB1 && t1 > magB)) {
CGFloat dot = [Vector dotProductA:_A B:[Vector vectorFromA:a0 B:pB]];
if (clampA0 && dot < 0) {
dot = 0;
} else if (clampA1 && dot > magA) {
dot = magA;
}
pA = [[PointCartesian alloc] initWithCoordinatesX:a0.x + _A.x*dot Y:a0.y + _A.y*dot Z:a0.z + _A.z*dot];
}
}
return [[[Vector alloc] initWithX:pA.x-pB.x Y:pA.y-pB.y Z:pA.z - pB.z] length];
}
#end
Vector.h:
#import <Cocoa/Cocoa.h>
#class PointCartesian;
#interface Vector: NSObject
#property CGFloat x;
#property CGFloat y;
#property CGFloat z;
#property NSString *name;
- (id)initWithX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z;
- (CGFloat)length;
- (void)normalize;
+ (Vector*)vectorFromA:(PointCartesian*)a B:(PointCartesian*)b;
- (PointCartesian*)toPoint;
+ (CGFloat)dotProductA:(Vector*)a B:(Vector*)b;
+ (Vector*)crossProductA:(Vector*)a B:(Vector*)b;
#end
Vector.m:
#import <Cocoa/Cocoa.h>
#import "Vector.h"
#import "PointCartesian.h"
#implementation Vector
- (id)initWithX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z {
_x = x;
_y = y;
_z = z;
return self;
}
- (CGFloat)length {
return sqrt(pow(_x,2)+pow(_y,2)+pow(_z,2));
}
- (void)normalize {
_x = _x / [self length];
_y = _y / [self length];
_z = _z / [self length];
}
+ (Vector*)vectorFromA:(PointCartesian*)a B:(PointCartesian*)b {
Vector *AB = [[Vector alloc] initWithX:b.x-a.x Y:b.y-a.y Z:b.z-a.z];
return AB;
}
- (PointCartesian*)toPoint {
return [[PointCartesian alloc] initWithCoordinatesX:_x Y:_y Z:_z];
}
+ (CGFloat)dotProductA:(Vector*)a B:(Vector*)b {
return a.x*b.x + a.y*b.y + a.z*b.z;
}
+ (Vector*)crossProductA:(Vector*)a B:(Vector*)b {
return [[Vector alloc] initWithX:a.y*b.z - a.z*b.y Y:a.z*b.x - a.x*b.z Z:a.x*b.y - a.y*b.x];
}
#end
main.m:
#import <Cocoa/Cocoa.h>
#import "PointCartesian.h"
int main(int argc, const char * argv[]) {
PointCartesian *a0 = [[PointCartesian alloc] initWithCoordinatesX:27.83 Y:31.74 Z:-26.60];
PointCartesian *a1 = [[PointCartesian alloc] initWithCoordinatesX:13.43 Y:21.77 Z:46.81];
PointCartesian *b0 = [[PointCartesian alloc] initWithCoordinatesX:77.54 Y:7.53 Z:6.22];
PointCartesian *b1 = [[PointCartesian alloc] initWithCoordinatesX:26.99 Y:12.39 Z:11.18];
CGFloat d = [PointCartesian shortestDistanceBetweenTwoSegmentsA0:a0 A1:a1 B0:b0 B1:b1];
NSLog(#"%f", d);
return NSApplicationMain(argc, argv);
}
output:
2020-06-26 17:57:30.762912+0200 Distance[2235:111429] 15.651394
The distance should be 15.826771412132246 according to the python code in link above.